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APPENDIX B - ROBSIM PROGRAMMER’S GUIDE 


Introduction 


The purpose of the Robotic Simulation (ROBSIM) program developed at Martin 
Marietta Denver Aerospace is to provide a broad range of computer capabilities 
to assist in the design, verification, simulation, and study of robotic sys- 
tems* ROBSIM is programmed in Fortran 77 and implemented on a Digital Equip- 
ment Corporation VAX 11/750 computer using the VMS operating system* 

This programmer's guide describes the ROBSIM implementation and program 
logic flow, and the functions and structures of the different subroutines. 

With this manual and the in-code documentation, an experienced programmer can 
incorporate additional routines and modify existing ones to add desired cap- 
abilities. 

This appendix is separated into four main sections. The following section 
presents an overview of the ROBSIM implementation and should be fully under- 
stood before attempting to modify the program. The remaining sections describe 
the primary ROBSIM executable images: (1) INITDRVR - the system definition 
function driver, (2) SIMDRVR - the analysis tools driver, and (3) POSTDRVR - 
the postprocessor function driver. Each of these three sections contains a 
block diagram showing the subroutines included in the corresponding executable 
and program logic flow. This is followed by functional descriptions and visual 
control logic representations (VCLRs) for these subroutines. Items (2) and (3) 
will be included in the final submission. 

VCLR format . - VCLR diagrams present program logic flow in a format that 
is compatible with structured programming. The use of VCLR diagrams offers 
several advantages over the use of flow charts: 

1) Only the standard constructs are used; 

2) The total scope and impact of the logic can be seen and easily under- 
stood; 

3) No extraneous symbols, connections or notations are used. 

VCLR provides a picture of a software design. It enables software engi- 
neers to express their thinking visually and stresses the control logic of the 
design. 

Standard constructs in visible control logic representations are the same 
as those for pseudocode: SEQUENCE, IETHENELSE, DOWHILE, DOUNTIL, and DOCASE; 
only the representations differ. 

SEQUENCE: A SEQUENCE is simply one standard construct or one single 

statement followed by another. If PI and P2 are standard constructs or single 
statements, the sequence would appear in a visible control logic representation 
as 


B-l 


PI 


P2 


IFTHENELSE: IFTHENELSE consists of a true/ false test and a path for each 

state. The true path appears on the left side, under the "T." One of the 
paths may be a "do nothing" or "NULL" path. One or both paths must consist of 
a standard construct or of a single statement. If "Cl" is the condition being 
tested, "PI" is the true path, and "P2" is the false path, the IFTHENELSE con- 
struct would be written as 



DOWHILE: The DOWHILE is a loop with these characteristics: 

1) The counter or other item to be "incremented" is initialized before 
entering the loop; 

2) The test is performed at the beginning of the loop. The conditions 
that must exist for the loop to be executed are the conditions that 
appear in the DOWHILE test; 

3) The item to be executed must be a standard construct or a single 
statement ; 

4) The counter is incremented or other increment— like action is generally 
taken (e.g., another line is read) at the end of the loop. 

If "Cl" is the condition that must exist for the loop to be executed, and 
"Pi" is a standard construct or single statement, the DOWHILE would be written 


DOWHILE Cl 


PI 
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DOUNTIL: The DOUNTIL is a loop with these characteristics: 

1) The counter or other item to be "incremented" is initialized before 
entering the loop; 

2) The test is performed at the end of the loop. The conditions that 
must exist to exit from the loop are those that appear in the DOUNTIL 
test; 

3) The item to be executed must be a standard construct or a single 
statement; 

4) The counter is incremented or other increment-like action is generally 
taken (e.g., another line is read) at the beginning of the loop. 

If "Cl" is the condition that must exist to exit from the loop and "PI" is 
a standard construct or single statement, the DOWHILE would be written as 


PI 


DOUNTIL Cl 


DOCASE: The DOCASE construct is for executing a different set of state- 

ments for each of several different values of a variable. If "Cl" is the var 
iable being tested and if "Cl" may have values 1, 2, or 3, the construct ap- 
pears 



Example A 


X \ 


Cl=l 

A 


X \ 

Cl=2 




T Cl 

na 

PI 

P2 

P3 

P4 


Example B 


Example A is equivalent to the nested IFTHENELSE form shown in B. 

Subroutine indexing . - The subroutine descriptions and VCLRs are arranged 
according to the number assigned in the block diagram. This label consists of 
three parts (nl.n2.n3). The first part (nl) indicates with which executable — 
(1) INITDRVR, (2) SIMDRVR, or (3) POSTDRVR — the module is associated. While 
some routines are used in more than one executable, each is documented only 
once and labeling number nl tells which section includes that documentation. 


RECEDING PAGE BLANK NOT FILMED 
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The next number (n2) indicates the level in the program hierarchy at which the 
routine occurs. There are three main levels under each executive driver and a 


fourth level that is assigned to the utility functions used by a variety of 
routines. The final number (n3) in the routine label distinguishes the modules 
within each level of one executable program. 



In-code documentation. — Although the information contained in this pro— 
grammer 1 s guide provides an understanding of the overall program logic and 
function of the individual subroutines, the bulk of the program documentation 
is included in the Fortran program modules. This enhances the accessibility of 
the documentation and allows it to be updated as modifications are made. Each 
Fortran module contains a preamble that lists the routine's: 


1) 

Purpose; 


2) 

Input (calling arguments and 

terminal inputs); 

3) 

Output (calling arguments and 

terminal outputs); 

4) 

Common variables; 


5) 

Internal variables; 


6) 

External references; 


7) 

Functional description; 


8) 

Assumptions and limitations; 


9) 

Special comments; 


10) 

References . 




Figure B-l illustrates an example of this in-code documentation. The file 
SKLTN.FOR contains a skeleton of the preamble for use in writing new programs. 


W- ' ’ 
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QKl&tv -.L 

OF POOR QL’ALJT^ 



CDO 

COO ROBOTICS SIMULATION (ROBSIM) PROGRAM 
CDO SYSTEM DEFINITION FUNCTION ROUTINE 
COO 


CD l 
CDI 
COL 
CDI 
CDI 
CDI 
CDI 
CDI 
CDI 
CDI 
CDI 
CDI 
CDI 
CDI 
CDI 
C*** 
CD2 
CD2 
CD2 
CD2 
CD2 
CD2 
CD2 
CD2 
CD2 
CD2 
CD2 
CD2 
CD2 
CD2 
CD2 
CD2 
CD2 
CD2 
CD2 
CD2 
CD2 
CDS 
CD2 
CD2 
CD2 
CD2 
CD2 


PURPOSE 

The purpose of Che ROBSIM Program is Co provide a 
broed range of computer capabilities to assist in the 
design, verification, s imitation, and study of 
robotic systems. 

The progrsa INITDRVR is Che Systea Definition Function 
driver. It operates in an interactive node, proapting the 
user for the systea definition option desired. Valid options 
are: create or aodify an arm data file, create or 
aodify a detailed environment file, create or aodify load 
objects, create a systea data file, or torainate INITDRVR 
progrsa execution. 


DEFINITION OF INPUT 


CALLING ARGUMENTS 

SYMBOL TYPE DIM DEFINITION 

N/A 

TERMINAL INPUTS 

SYMBOL TYPE DIM DEFINITION 

IMODE 1*4 l Select fleg for node of operation 

• I, Create/aodify an arm data file 

• 2, Create/aodify detailed 

environaent file 

“ 3, Create/aodify load objects data file 

- 4, Create systea date file 

- 5, End ROBSIM INITDRVR executive 

I TERM 1*4 l Progrea terainetion flag, 

requested following fetal 
error 

■ 1, Rs issue progrea aode 
selection proapt 

• RETURN, Terainate progrea 


c +****a**a ********************************** 

CD3 

CD3 DEFINITION OF OUTPUT 
CD3 

CD3 CALLING ARGUMENTS 

CD3 

CD3 SYMBOL TYPE DIM DEFINITION 

CD3 

CD3 N/A 

CD3 

CD3 TERMINAL OUTPUTS 

CD3 

CD3 SYMBOL TYPE DIM DEFINITION 

CD3 

CD3 None 

CD3 


CD4 

CD4 COMMON VARIABLES 
CD4 


CD4 INPUT 


CD4 

CD4 

CD4 

CD4 

CD4 

CD4 

CD4 


LUl, LU2 
OUTPUT 
NONE 


CD5 

CD5 

CD5 

CD5 

CDS 

CU5 

CD5 

CD5 

CDS 

CD5 

CDS 

CDS 

CDS 

CDS 

CDS 

CD5 

CDS 

CD5 


INTERNAL VARIABLES 

SYMBOL TYPE DIM DEFINITION 


IERROR 1*4 

IHLP 1*4 

MODE 1*4 


1 


1 


1 


Error indicator flag 

*0, No errors encountered 
•NE. 0, Error has occurred, 
contains appropriate 
error number required 
by routine ERRMSG 

The integer 911 input froa terminal to 
signify user wishes to access help file 
ROBSIM program mode flag, sec to l 
to indicate program execution is 
currently within the System Definition 
Function 


CD6 

CD6 EXTERNAL REFERENCES 
CD6 

CD6 I/O FILES 

CDS 

CDS LUl - Logical unit sssignsd for input froa the terminal 

CD6 LU2 - Logical unit assigned for output to Che terminal 

CDS 

CD6 SCRATCH FILES 

CD6 

CD6 N/A 

CD6 

CDS EXTERNAL ROUTINES 

CD6 

CD6 BLDARM - Create/aodify era data fils 

CD6 BLDENV - Crests/aodify detailed environaent file 

CDS BLDLOD - Create/aodify load objects data fils 

CDS BLDSYS - Crests systea data file 

CD6 ERRMSG - Prints error messages for any error occurring 

CDS during execution 

CD6 LBR_HELP - Gains access to the ROBSIM help library 

CD6 s£TLU - Default logical units routine 

CD6 

C ************************************************************* 

CD7 

CD7 FUNCTIONAL DESCRIPTION 
CD7 

CD7 The ROBSIM command fils prompts the user for the 

CD7 progrsa function desired. The three ROBSIM progrea 
CD7 functions are System Definition, Analysis Tools, and 
CD7 Post Processing. The user may also rsquest program 
CD7 termination. Upon receiving a valid function request, 

CD7 the ROBSIM coaaand file transfers control to and executes 
CD7 the appropriate function driver. 

CD7 The System Definition executive calls BLDARM, BLDENV, 

CD7 BLDLOD or BLDSYS to create or modify an arm, environment, 

CD7 load objects or arm system. 

CD7 


CDS 

CDS 

CDS 

CDS 

CDS 

CDS 

CDS 

CDS 

CDS 

CDS 

CDS 

CDS 

CDS 


ASSUMPTIONS AND LIMITATIONS 

1. ROBSIM is programed in FORTRAN 77 for use on 
VAX 11/780 computers under the VMS operating 
systea. 

2. ROBSIM uses Evans and Sutherland Multi Picture 
System graphics using MPS FORTRAN callable 
graphics routines. Use of the graphics 
capabilities in ROBSIM is optional, however 
full utilisation of the program capabilities 
is greatly limited without Che graphics. 


C'**** 

CD9 

***********, ***************** 


CD9 

SPECIAL COMMENTS 


CD9 



CD9 

1. If graphics is desired, 

, the graphics work station 

CD9 

must be se signed using 

individual facility 

CD9 

procedures. 


CD9 

2. The necessary arm data 

files must exist prior to 

CD9 

building a system. 


CD9 

ASAXAASSAS....A..AAAlAlAASiSlAAlJ 

uxumunuiJiimxmnM 


CD 10 
CD10 
CD10 
CDIO 
CDIO 


REFERENCES 

None 


CDX1 INITDRVR 

CDX THIS MODULE IS EXECUTED BY THE SYSTEM DEFINITION PROCESSOR 
CDX 

CDX2 PARAMETERS 
CDX Qualifiers: 

CDX /IMODE 
CDX3 /IMODE 
CDX TYPE 

CDX 1*4 

CDX 
CDX 
CDX 
CDX 
CDX 
CDX 

CDX2 FUNCTION 

CDX The ROBSIM command file prompts the user for the 

CDX program function desired. The three ROBSIM program 
CDX functions are System Definition, Analysis Tools, and 
CDX Post Processing. The user may also request program 
CDX termination. Upon receiving a valid function request, 

CDX the ROBSIM coMnd file transfers control to and executes 
CDX the appropriate function driver. 

CDX The System Definition executive cells BLDARM, BLDENV, 

CDX BLDLOD or BLDSYS to create or modify an arm, environment, 
CDX load objects or arm system. 

CDX 


DIM DEFINITION 

1 Select flag for aode of operation 

• 1, Create /modify an arm data file 

• 2, Creete/modify detailed 

environment file 

■ 3, Creete/modify load objects data file 

• 4, Create system data file 

- 5, End ROBSIM INITDRVR executive 


Figure B-l.- 


Example of in-code 


documentation. 
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Implementation Notes 


This section describes the programming conventions used in implementing 
ROBSIM on the VAX-11 computer architecture under the VMS operating system. 

The program consists of a large number of Fortran routines and their compiled 
object modules, along with a limited number of executable images and VMS 
command files. 

Executive— level command file. — The executive level of ROBSIM is handled 
by an interactive command file ROBSIM.COM. Figure B-2* shows this command 
file. This file runs one of the ROBSIM executable images (Fig. B-3) selected 
by the user. INITDRVR.EXE is the executable containing the system definition 
routines and SIMDRVR.EXE contains the analysis tools image. The postprocessor 
functions reside in two executable files: (1) POSTDRVT.EXE for video- terminal 
display of results, and (2) POSTDRHP.EXE for hardcopy plotting. This is be- 
cause the display software requires linking of different modules for terminal 
vs hardcopy displays* 



Figure B-3.- ROBSIM executive command file. 


Linking the programs . - Each executable image contains an executive rou- 
tine having the same name as the executable (POSTDRVR for the postprocessor 
images) and a large number of supporting routines. To facilitate linking, the 
compiled object modules are included in object libraries. The programs are 
linked by executing command files that reference these libraries. Figure B-4 
shows the linker command files and Table B-I lists the programs in the object 
libraries. 

The bulk of the libraries are contained in the main ROBSIM directory 
(ROBSIM DIR:) but some reside in different directories or devices. This is 
especially convenient for implementations on multiple-disk systems. Table B-II 
lists the alternate device specifications; these logicals must be assigned to 
appropriate physical devices, possibly during the log-in procedure. 


* Full-page figures can be found at the end of this section. 



TABLE B-I. MODULES IN ROBSIM OBJECT LIBRARIES 


CNTLLIB.OLB 

OBJECT LIBRARY 

MODULES : 

CMPCTRL 

CNTRSIG 

CONTROL 

FORREF 

FORTOR 

ICVTATD 

PIDCON 

PIDFOR 

PIDINIT 

POSSENS 



CRLIB.OLB OBJECT LIBRARY MODULES: 

BLDARM 

BLDDAT 

BLDENV 

BLDLOD 

BLDSYS 

CREATARM 

DEFSPJT 

ENVIR 

LOAD 

RDARM 

RDARMS 

RDENV 

RDENVS 

RDLOAD 

RDLODS 

TOTMAS 

WRTARM 

WRTENV 

WRTLOD 

WRTSYS 


EANDSLIB.OLB OBJECT LIBRARY MODULES: 

DATOUT 

DIALS 

ESCNTRL 

ESPAUS 

FORM 

GRAF IX 

ISBIT1 

LOGO 

POSTGRAF 

ROTMAT 



GEOMLIB.OLB 

OBJECT LIBRARY 

MODULES : 

ACTUATOR 

BASE 

BASES 

JOINT 

LINK 

LOCMOD 

OBJECT 

SPAN 

TLDMAS 

TOOLJT 

TOOLLK 


GRAFLIB.OLB 

OBJECT LIBRARY 

MODULES : 

CYL 

DATATAB 

DBAS 

DRAW 

DRWENV 

DRWLOD 

ESMAT 

FILLET 

GRAFIX 

GRAPH 

GRINIT 

GRTERM 

MAT 

MATVEC 

OBSTCL 

ORIENT 

POSTGRAF 

RECT 

SYSGRAF 

TRISTR 


MATHLIB.OLB 

OBJECT LIBRARY 

MODULES : 

CETM 

CRPD 

DOT2 

GAUSS 

MATMPY 

REPCOL 

SLVLIN 

SLVLIN2 

SOLVE 
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TABLE B— I. (concl) 



POSTLIB.OLB 

OBJECT LIBRARY 

MODULES : 

AXES 


HDWMOTIN 

LDTHET 

MINMAX 


ROBPLT 

SCAL 

SIMOTION 


TICMRK 


REQLIB.OLB 

OBJECT 

LIBRARY 1 

MODULES : 

ACTORQ 


CABSM 

CHKLMT 

CNSTFOR 


CNTRLR 

CVTIN 

DEBUG 


DYNAM 

ENDREQ 

FORCE 


FTIN 

JTPOS 

LODMOV 


ORERR 

OUTREQ 

PCNTRL 


POSGRDJT 

POSSPJT 

PTACC 


PVASPJT 

RATEPRO 

RCICR 


RCNTRL 

REQOPT 

REQPLT 


REQPRT 

REQSOF 

REQTRQ 


REQUIR 

SEGINIT 

SGMNT 


SPRGFOR 

SPRGINC 

TORQUE 


VAGRDJT 


SETLIB.OLB 

OBJECT 

LIBRARY 

MODULES : 

ADDMAS 


ADDMAS 2 

BASPUT 

GRASP 


RCICR 

RDSYS 

RELEAS 


SETUP 

SETUP2 

SIMLIB.OLB 

OBJECT 

LIBRARY 

MODULES : 

ACTIVPIH 


CALC I 

DEFCNST 

DERIV 


DOT 

DRTORQ 

EFINRT 


EFINRT2 

ENDSIM 

INITCO 


INTGRT 

LDVOLT 

NLINK 


OUTSIM 

PRTARM 

RESPON 


SETCNST 

SIMLMT 

SIMOPT 


SIMPLT 

SIMPRT 

SLVTHDD 


STOPFR 


UTILLIB.OLB OBJECT LIBRARY MODULES: 

CVTUNIT 


DEFUNIT 

ERRMSG 

JACOB 


LININT 

OUTUN 

SETLU 


ZERCOM 
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TABLE B— II# - DEVICE -DIRECTORY SPECIFICATIONS IN ROBSIM 


Logical 

Modules contained in directory 

ROBSIM DIR: 

Basic ROBSIM modules 

MPS DIR: 

Evans and Sutherland graphics modules 

HELP DIR: 

Help utility modules 

DI3000_DIR: 

Display modules (video-terminal and 


hardcopy) 

HARDCOPYJDIR: 

Modules for creating meta-files from 


picture files 


Fortran files and object files , - Each Fortran routine is included in its 
own file; the name of the file is that of the routine it contains and the file 
type is "•FOR". An object file with the same name and type "•OBJ 11 holds the 
compiled version of each Fortran file. After a Fortran module is modified, an 
executable image containing the updated version can be obtained by issuing the 
following commands: 

FORTRAN MODULENAME 

LIBRARY/ REPLACE LIBNAME MODULENAME 

@LNKNAME 

Command files FORROB.COM and REPLIB.COM exist for compiling the entire 
set of routines in the main directory and updating the object libraries. 
PRTROB.COM provides for printing all of the Fortran modules. Each of these com- 
mand files should be updated when routines are added to or deleted from the 
program. 

Fortran COMMON blocks . - The variables used by several routines are ar- 
ranged into COMMON blocks. The text file ROBCOM.DOC lists and briefly de- 
scribes the variables included in each COMMON block. A Fortran COMMON state- 
ment for each block resides in a file of type ".CMN" that has the same name as 
the COMMON block. The COMMON blocks are included during compilation of the 
Fortran modules using the INCLUDE statement. This allows a block to be modified 
by changing only the ".CMN" file, instead of all the Fortran modules that use 
this block. 

During compilation of the Fortran modules, maximum values must be speci- 
fied for some of the array dimensions. These maximum dimensions are often de- 
fined by PARAMETER statements, and most of these statements are included in 
the file MXPRMS.TXT. Figure B-5 shows a listing of this file. To change the 
maximum dimension of some variables (e.g., to increase the number of arms pos- 
sible in a system), the programmer must only change the appropriate parameter 
(MXARMS) and recompile the programs. 
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MXPRMS.TXT 

THE PARAMETERS IN THIS INCLUDE FILE ARE CONSTANT VALUES REQUIRED 
BY THE PROGRAM. 

PARAMETER (MXLNKS-10,MXARMS-2) 

PARAMETER (MAXORD-3,MAXSEG-20,MXPLTS«31) 

PARAMETER (MXENCMPS-30,MXGRCMP-20,MXLDCMPS-10) 

PARAMETER (MXPTS-IO.MXLODS-IO) 


DEFINITIONS 

SYMBOL TYPE 

MXARMS 1*4 
MXLNKS 1*4 
MXPLTS 1*4 

MAXORD 1*4 
MAXSEG 1*4 

MXPTS 1*4 
MXGRCMP 1*4- 
MXENCMPS 1*4 
MXLDCMPS 1*4 
MXLODS 1*4 


DIMENSION 

1 

1 

1 

1 

1 

1 

1 

1 

1 

1 


DEFINITION 

MAXIMUM NUMBER OF ARMS 
MAXIMUM NUMBER LINKS ALLOWABLE 
MAXIMUM NUMBER OF Y ARRAY DATA 
PARAMETERS WHICH MAY BE 
WRITTEN TO PLOT FILE 
ORDER OF THE POLYNOMIAL DESCRIBING 
THE MOTION TIME HISTORY 
MAXIMUM NUMBER OF TIME SEGMENTS 
ALLOWED TO DESCRIBE THE MOTION 
TIME HISTORY 

MAXIMUM NUMBER OF POINT MASSES 
IN EACH LINK OR LOAD 
MAXIMUM NUMBER OF GRAPHICS COMPONENTS 
ALLOWED PER LINK 

MAXIMUM NUMBER OF GRAPHICS COMPONENTS 
IN ENVIRONMENT 

MAXIMUM NUMBER OF GRAPHICS COMPONENTS 
IN EACH LOAD OBJECT 

MAXIMUM NUMBER OF LOAD OBJECTS ALLOWED 


Figure B— 5. — Listing of MXPRMS.TXT. 


Interactive help utility. - An interactive help utility is implemented in 
ROBSIM to provide online assistance to the user for answering some of the pro- 
gram prompts. The utility provides the user with information on the function 
and form of the routine and its arguments. The utility is implemented using a 
mixture of custom software and the VMS help utility. Information for the help 
library is included in the in-code documentation in the Fortran modules under 
the heading "CDX". The command file MAINHLP.COM is executed to set up a help 
library ROBLIB.HLB from this documentation. The executable image MNEXTRACT.EXE 
is run to extract the help library information from the Fortran. modules ; it 
selects all program lines beginning with ''CDX” and deletes the CDX headings. 
The formatted file MAINHLP.DOC lists the Fortran modules (type ’’.FOR" implied) 
to be searched for help library information. All segments that are extracted 
must follow the conventions for creating help libraries as described in VAX/VMS 
Volume 4A Program Development Tools Utilities Reference Manual , Section 10.3.2. 
They are temporarily stored in a file of type .HLP . 
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(Warning - all 11 • HLP" routines are deleted by the coraraand file execution!) 

As an example, Figure B-6 shows the help documentation extracted from pro- 
gram POSTDRVR. The help utility is accessed within the ROBSIM program modules 
by a call to the subroutine LBR_HLP. This module is included in the object 
library HELP_DIR:QESTLIB.OLB along with the other routines needed for the help 
utility. Table B-III summarizes the main files employed by the help utility. 


1 POSTDRVR 

THIS MODULE IS EXECUTED BY THE POST PROCESSOR 

2 PARAMETERS 

Qualifiers: 

/IMODE 

3 /IMODE 

TYPE DIM DEFINITION 

1*4 1 Select flag for mode of operation 

= 1, Replay simulation graphic motion 
only 

= 2, Replay simulation versus hardware 
motion 

= 3, Parameter versus parameter plots 
= 4, Return to ROBSIM executive 

2 FUNCTION 

The result of executing option 1, is to call subroutine 
SIMOTION which provides a replay of the robotic system motion 
produced from a simulation run. Option 2 provides a 
comparison of motion resultant from direct hardware theta value 
read and motion resultant from simulation execution, through 
subroutine HDWMOTIN. 

If option 3 is selected, subroutine ROBPLT is called to 
provide parameter versus parameter plots of any of the 
data computed and written to a plot file during the 
Requirements Analysis Tools Function. 

Option 4 returns execution to the primary ROBSIM level. 


Figure B-6. - Listing of help documentation extracted from POSTDRVR. 


TABLE B-III. - FILES USED FOR INTERACTIVE HELP UTILITY 


MAINHLP.COM 

MAINHLP.DOC 

MODULENAME.HLP 

ROBLIB.HLB 
LIB_HELP.FOR 
HELP MAC .MAR 


Executive command file for extracting help 

library documentation from FORTRAN code 

Names of FORTRAN modules containing help information 

Temporary file of help documentation extracted 

from routine MODULENAME • FOR 

Data file used for help utility 

Module containing program which reads ROBLIB.HLB 
Macro routine used to access help facility 
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Hardcopy utility * — The ROBSIM program provides the capability for inter— 
active display of the manipulator system on an Evans and Sutherland graphics 
workstation during system creation or analysis. In addition, plots of this dis- 
play can be generated on a hardcopy plotter for future reference. Generation 
of the hardcopy plot entails three steps: (1) creation during program execution 
of a picture file, (2) conversion of this file into a graphics meta-file, and 
(3) translation of this meta-file into a display or plot. 

The first step is initiated by a call to the routine HARD_COP at the 
points in the program where a hardcopy of the E&S display may be desired. If 
the user selects to keep a hardcopy of the current display, a picture file 
named by the user is created. The routines for this procedure are in object 
libraries HCPIC.OLB and HCMFL.OLB in directory HARDCOPY_DIR: and are linked 
with the ROBSIM executable images (Fig. B-4). Program execution continues 
after the meta-file is completed. 

After the ROBSIM run terminates, the user can activate programs that 
convert the picture files into meta-files by executing HCMFL.EXE. The re- 
sulting picture file can be translated into a display on the video-terminal or 
hardcopy plotter using DI3000 software. The images VTMETTRNS.EXE and 
HPMETTRNS.EXE in device-directory METTRNS_DIR: perform this translation. 
Activating these images is made easier by assignments in the log-in command 
file: 


HCMFL := RUN HARDCOPY_DIR: HCMFL.EXE 

HPMETTRNS := RUN DI3000_DIRrHPMETTRNS.EXE 
VTMETTRNS := RUN DI3000_DIR: VTMETTRNS .EXE 

The user need only type the keyword to start execution of these programs. 

File "type 11 conventions . - The different types of files used in creating 
the ROBSIM program are designated by individual file-type suffixes in their 
file specifications. It is recommended that the programmer and user maintain 
these conventions in the files they create. Table B— IV lists the suggested 
type specifications. 
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TABLE B-IV.- FILENAME CONVENTIONS USED IN ROBSIM 


appendage 

FOR FILENAME 

DEFINITION 

.ARM 

ARM GEOMETRY FILE CREATED 
DURING SYSTEM DEFINITION 

.SYS 

SYSTEM GEOMETRY FILE CREATED 
DURING SYSTEM DEFINITION 

.LOD 

LOAD GEOMETRY FILE CREATED 
DURING SYSTEM DEFINITION 

.ENV 

ENVIRONMENT GEOMETRY FILE CREATED 
DURING SYSTEM DEFINITION 

.OBS 

OBSTACLE FILE (NONPLANAR- X,Y,Z COORDINATES) 
READ FOR DETAILED GEOMETRY INPUT 

• ACT 

ACTUATOR DEFINITION INPUT FILE 
READ BY MODULE ACTUATOR DURING 
SYSTEM DEFINITION 

.CON 

RESPONSE ANALYSIS CONTROL OPTIONS 
INPUT FILE READ BY CONTRL MODULE 

• THT 

HARDWARE THETA ANGLE INPUT FILE 
CREATED FROM HARDWARE CONVERSION 
ROUTINE, AND INPUT DURING POST PROCESSING 

• VLT 

HARDWARE VOLTAGE CONTROL SIGNAL INPUT 
FILE CREATED FROM HARDWARE CONVERSION 
ROUTINE, AND INPUT DURING RESPONSE 
ANALYSIS EXECUTION 

• THP 

TIME HISTORY PROFILE FILE CREATED BY 
AND INPUT TO REQUIREMENTS ANALYSIS 

.DAT 

REQUIRMENTS OR RESPONSE SIMULATION 
OPTIONS INPUT FILES; ALSO SOME OUTPUT FILES 

.CMN 

COMMON BLOCKS INCLUDED THROUGHOUT PROGRAM 

.OLB 

PROGRAM LIBRARY FILES 

.LIS 

LISTINGS OF SUBROUTINES IN EACH LIBRARY 

.FOR 

FORTRAN CODE 
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TABLE B-IV . - (concl) 


.OBJ 

FORTRAN OBJECT MODULES 

.EXE 

ROBSIM PROGRAM AND UTILITIES EXECUTABLES 

.COM 

COMMAND FILES FOR COMPILING, REPLACING 
MODULES IN APPROPRIATE LIBRARIES, 
LINKING THE DRIVERS, PRINTING ALL 
MODULES, AND RUNNING THE PROGRAMS 

.TXT 

PARAMETER FILES INCLUDED IN MODULES 
THROUGHOUT PROGRAM 

.DOC 

DOCUMENTATION FILES 

.HLP 

USER HELP FILES GENERATED WITH THE 
MNEXTRACT UTILITY IN HELPER DIRECTORY 
ACCESSIBLE WITH THE LBR HELP UTILITY 

• PRT 

SIMULATION PRINT OUTPUT FILES 

• PLT 

PLOT OUTPUT FILES FOR HEWLETT PACKARD 
X-Y PLOTTER OR VT125 GRAPHICS TERMINALS 

.AGF 

ARM GEOMETRY PRINT OUTPUT FILES CREATED 
DURING SYSTEM DEFINITION 

.PIC 

PICTURE FILES OF EVANS AND SUTHERLAND 
DISPLAYS, GENERATED WITH HARD COP 
ROUTINE; MAY BE REPRODUCED ON THE 
HEWLETT PACKARD PLOTTER AFTER 
CONVERSION TO META-FILE FORMAT 

.SOF 

SIMULATION OUTPUT FILE FOR POST PROCESSING 

.AVT 

ACCELERATION-VELOCITY-THETA OUTPUT FILE 

.TRQ 

TORQUE OUTPUT FILE 

.OUT 

ACTUAL HARDWARE OUTPUT FILES FOR 
VOLTAGE CONTROL SIGNALS AND CORRESPONDING 
THETA ANGLE VALUES 
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ORIGINAL P/ a: !3 
OF POOR QUALITY 


lROBSXM.COM 
(SET NOVERIFY 
(SET TERM/ NO BROAD 
(COUNT-O 
(LOOPS: 

(COUNT-COUNT+1 

(IF COUNT. GT.l THEN GOTO ASKNEXT 
(ASKWICH: 

$ WRITE SYS(OUTPUT "INPUT (INITDRVR)-- 
(WRITE SYS(OUTPUT " (SIMDRVR) — 


$ WRITE SYS(OUTPUT " (POSTDRVR) — 


TO RUN ROBSIM SYSTEM 
DEFINITION FUNCTION" 
TO RUN ROBSIM 
SIMULATION ANALYSIS 
TOOLS FUNCTION" 

TO RUN ROBSIM POST 
PROCESSOR FUNCTION" 


(PROMPT: -WHICH: 

(READ/ PROMPT-"' 'PROMPT'" SYS (COMMAND WHICH 
(IF WHICH. EQS. "SIMDRVR" THEN GOTO SIM 
(IF WHICH. EQS. "POSTDRVR" THEN GOTO POST 
(INIT: 

(ASSIGN TT SYS$INPUT 
(ASSIGN rr SYS(OUTPUT 

(RUN SYS(SYSDEVICE: [ROBSIM. MARK] INITDRVR 
(DEASSIGN SYS(INPUT 
(DEASSIGN SYS(OUTPUT 
(INQUIRI : 

(PROMPT: -INPUT (Y) TO RUN SYSTEM DEFINITION FUNCTION AGAIN, - 
(OTHERWISE, RETURN) 

(READ/ PROMPT- ,,,, PROMPT'" SYS(COMMAND WHICH 
(IF WHICH. EQS. "Y" THEN GOTO INIT 
(GOTO LOOPS 
(SIM: 

(ASSIGN TT SYS(INPUT 
(ASSIGN XT SYS(OUTPUT 

(RUN SYS(SYSDEVICE: [ROBSIM. MARK] SIMDRVR 
(DEASSIGN SYS(lNPUT 
(DEASSIGN SYS(OUTPUT 
(INQUIRS : 

(PROMPT: -INPUT (Y) TO RUN SIMULATION ANALYSIS TOOLS - 
FUNCTION AGAIN, (OTHERWISE, RETURN) 

(READ/ PROMPT-"" PROMPT'" SYS(COMMAND WHICH 
(IF WHICH. EQS. "Y" THEN GOTO SIM 
(GOTO LOOPS 
(POST: 

(WRITE SYS(OUTPUT "DO YOU WISH (1) TERMINAL OR 

(2) HARDCOPY PLOTTING?" 


(PROMPT:- ENTER INTEGER: 

(RE AD/ PROMPT-" "PROMPT"' SYS(COMMAND WHICH 
(ASSIGN TT SYSdNPUT 

(assign tt sYS(otrrpur , 

(IF WHICH. EQS. "I" THEN RUN SYS$SYSDEVICE: [ ROBSIM. MARK] POSTDRVT 
(IF WHICH. EQS. "2" THEN RUN SYS( SYSDEV ICE :[ ROBSIM. MARK] POSTDRHP 
(DEASSIGN SYS(INPUT 
(DEASSIGN SYS$OUTPUT 
(INQUIRP : 

(PROMPT: -INPUT (Y) TO RUN POST PROCESSOR FUNCTION AGAIN, - 
(OTHERWISE, RETURN) 

(READ/ PROMPT-"" PROMPT'" SYS (COMMAND WHICH 
(IF WHICH. EQS. "Y" THEN GOTO POST 
(GOTO LOOPS 
(ASKNEXT: 

(PROMPT: -INPUT (Q) IF YOU WISH TO EXIT THE PROGRAM 
* (OTHERWISE. RETURN) 


(READ/ PROMPT*"' 'PROMPT'" SYS(COMMAND QUIT 
(IF QUIT. EQS."" THEN GOTO ASKWICH 
(EXIT 
(STOP 


Figure B-2.- The ROBSIM executive command file. 


LNKXNIT.COM ... 

$LINK/EXECUTABLE»INITDRVR INITDRVR,- 
CRLIB/LIB,- 
GEOMLIB/LIB,- 
GRAFLIB/LIB 
SETLIB/LIB,- 
EANDSLXB/LIB , - 
UTILLIB/LIB , - 
MATHLIB/LIB > - 
HCPIC/LIB,- 
HCMFL/LIB,- 
DRAO : [MPSGSP JMPLIB/L,- 

SYS$SYSDEVICE: [ROBSIM.HELPER]QESTLIB/LIB 


LNKSIM.COM 

$LINK/ EXECUTABLE*S IMDRVR SIMDRVR,- 
SIMLIB/LIB,- 
CNTLLIB/LIB , - 
REQLIB/LIB , - 
EANDSLIB/LIB 
UTILLIB/LIB , - 
SETLIB/LIB 
MATHLIB/LIB ,- 
HCPIC/LIB , - 
HCMFL/LIB , - 

DRAO: [MPSGSP jMPLIB/L,- 

SYS$SYSDEVICE: [ROBSIM. HELPER] QESTLIB/LIB 
LNKPOSTVT.COM 

$LINK/EXECUTABLE*POSTDRVT POSTDRVR,- 
POSTLIB/LIB,- 
SETLIB/LIB, - 
UTILLIB/LIB , - 
MATHLIB/LIB , - 
EANDSLIB/LIB ,- 
HCPIC/LIB »- 
HCMFL/LIB ,- 

DRAO: [MPSGSPjMPLIB/L,- 

SYS GR:DILIB/LIB/INCLUDE-(Q3ITOA,Q3ATOI) ,- 
METGEN/LIB , - 

SSLIB/ LI B , SYS GR: vt 125ddr/LIB, - 
VAXLIB/ LIB , - 

SYS$SYSDEVICE : [ ROBS IM. HELPER jQESTLIB/LI B 


LNKPOSTHP.COM 

$ LINK/ EXEC UTABLE*POSTDRHP POSTDRV R , - 

POSTLIB/ LIB , - 
SETLIB/LIB , - 
UTILLIB/LIB, - 
MATHLIB/LIB, - 
EANDSLIB/LIB, - 
HCPIC/LIB, - 
HCMFL/ LIB , - 
DRAO: [MPSGSPJMPLIB/L,- 

SYS GR:DILIB/LIB/INCLUDE=(03ITOA,03ATOI) ,- 
METGEN/LIB, - 

SSLIB/ LIB , SYS GR : HP7DDR/LIB, - 
VAXLIB/ LIB , - 

SYS$SYSDEV ICE : [ ROBSIM. HELPERlQESTLIB/ LIB 


Figure B-4.- ROBSIM linker command files 
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TABLE B-V. 


PROGRAMS EMPLOYED IN INITDRVR 


1.0 

INITDRVR 

1.3.1 

GRINIT 

1.4.1 

CVTUNIT 



1.3.2 

SPAN 

1.4.2 

MATMPY 

1.1.1 

SETLU 

1.3.3 

BASE 

1.4.3 

ERRMSG 

1.1.2 

LBR HELP 

1.3.4 

OBJECT 

1.4.4 

ROTMAT 

1.1.3 

BLDARM 

1.3.5 

GRAPH 

1.4.5 

CETM 

1.1.4 

BLDENV 

1.3.6 

DIALS 

1.4.6 

LOGO 

1.1.5 

BLDLOD 

1.3.7 

JOINT 

1.4.7 

CRPD 

1.1.6 

BLDSYS 

1.3.8 

ACTUATOR 





1.3.9 

LINK 



1.2.1 

ZERCOM 

1.3.10 

DEFSPJT 



1.2.2 

DEFUNIT 

1.3.11 

TOOLJT 



1.2.3 

CREATARM 

1.3.12 

TOOLLK 



1.2.4 

RDARM 

1.3.13 

TOTMAS 



1.2.5 

BLDDAT 

1.3.14 

ADDMAS 



1.2.6 

WRTARM 

1.3.15 

ADDMAS2 



1.2.7 

ENVIR 

1.3.16 

RCICR 



1.2.8 

RDENVS 

1.3.17 

GRTERM 



1.2.9 

RDARMS 

1.3.18 

BASPUT 



1.2.10 

BASES 

1.3.19 

JACOB 



1.2.11 

SETUP 

1.3.20 

DATOUT 



1.2.12 

SETUP2 

1.3.21 

FORM 



1.2.13 

HARD COP 

1.3.22 

CYL 



1.2.14 

SYSGRAF 

1.3.23 

RECT 



1.2.15 

RDLODS 

1.3.24 

TRISTR 



1.2.16 

LOCMOD 

1.3.25 

DATATAB 



1.2.17 

WRTSYS 

1.3.26 

FILLET 



1.2.18 

RDENV 

1.3.27 

OBSTCL 



1.2.19 

DRWENV 

1.3.28 

ORIENT 



1.2.20 

WRTENV 

1.3.29 

MAT 



1.2.21 

RDLOAD 

1.3.30 

MATVEC 



1.2.22 

LOAD 

1.3.31 

DRAW 



1.2.23 

TLDMAS 

1.3.32 

ESMAT 



1.2.24 

DRWLOD 

1.3.33 

DBAS 



1.2.25 

WRTLOD 







1.0 


INITDRVR 


The program INITDRVR is the system definition function driver. It oper- 
ates in an interactive mode, prompting the user for the system defini- 
tion option desired — create or modify an arm data file, create or modify 
a detailed environment file, create or modify a load objects file, 
create a system data file, or terminate INITDRVR execution. Subroutine 
SETLU is called to set the Fortran logical units. The necessary simple 
cylinder or detailed single arm file must exist prior to building a 
system. 
















1.1.1 SETLU 


SETLU is called from the various executive drivers to set the Fortran 
logical unit number to be stored in COMMON block LUNITBK for reference 
by the rest of the ROBSIM program. After assigning the variables to 
consecutive unit numbers, the "unit open" flags are reset to indicate 
the units are not open (except the terminal read and write units). 


SUBROUTINE SETLU 


ASSIGN LU1 THRU LU20 SUCCESSIVE 
LOGICAL NUMBERS STRRTING WITH 5 

CLU1=5. - - - ] 


SET FLAGS FOR LU1 AND LU2 
INDICATING UNITS OPEN 


RESET FLAGS FOR REMAINING UNITS 
INDICATING UNITS NOT OPENED 


DISPLAY LOGICAL RSSIGNMENTS TO 
USER RND PROMPT FOR FLRG TO 

CONTINUE 
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1.1.2 LBR HELP 


Subroutine LBR_HELP is called to execute the help utility during a 
ROBSIM run. It uses the object file created from the macro HELPMAC.MAR 
and runs the system help utilities as required. 



(VCLR for LBR HELP not available.) 
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1.1.3 BLDARM 


ORIGIN F 13 

Of POOk QUALITY 

BLDARM is met when a selection of 1, to create or modify an arm data 
file, is entered from INITDRVR. The user choices for mode of operation 
are (1) create a simple cylinder arm data file, (2) modify existing arm 
data file, (3) enter detailed graphics data for arm (a simple cylinder 
file must already exist), or (4) terminate arm definition and return to 
the INITDRVR. For initial creation, option (1), subroutine ZERCOM is 
called to zero the COMMON locations and then CREATARM is called to build 
the new data file. For modification, RDARM and CREATARM are called when 
option (2) is requested. BLDDAT is responsible for the invention of de- 
tailed arm geometry. In all cases, WRTARM will be called to write the 
arm data COMMON information. 
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1.1.4 BLDENV 


The user has the capability with routine BLDENV to specify a detailed 
physical representation for the robotic environment* Components for 
the environment are defined as basic geometric shapes (cylinders, cones, 
rectangular solids, symmetric or nonsymraetric trapezoidal figures, tri- 
angular cross-sectional beams, fillet components, data tablet— defined 
entities and obstacles. 


SUBROUTINE BLDENV 



O R IENT TO SPECIFY COMPONENT POS. RNP 
CRI_I_ uRXtiN IN NQRL.P COORDS. 


OR. 


SET IFLflG BASED ON ICHNG INPUT FLRG 


DO 


while CONTROL DIALS ACTIVATED 


CRL.L _ DRWENV WITH IFLRG TO roRWNB U OR 
REP 1— ACE EXISTING COMPONENT 


IFLRG = 2 


(~pi_L PQGET TO STATUS CONTROL. DIRt— S EVENT 


PROMPT FOR ICHNG FLAG 


CQHPONPrr H-Fxa. - a 



oKiGtftAL F^r;;: 

OF POOR QUALITY 
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1.1.5 BLDLOD 


ORIGINAL T 
OF POOR Qj 


m 7 

s V 



Through routine BLDLOD, the user has the capability to specify a de- 
tailed physical representation for the robotic load objects to be used. 
Components for the load objects are defined as basic geometric shapes 
(cylinders, cones, rectangular solids, symmetric or nonsymmetric trape- 
zoidal figures, triangular cross-sectional beams, fillet components, 
data tablet-defined entities, and obstacles). This subroutine creates 
a new file, or modifies an existing file of load objects, and includes 
the capability to specify the detail at the first creation session for 
the load objects. 







1.1.6 BLDSYS 


BLDSYS reads individual arm data files into system COMMON blocks, modi- 
fies the base locations if desired, sets up the environment and load ob- 
jects, and then stores the system file. 


SUBROUTINE BLDSYS 



UKiGlNAL PAGE IS 


OF POOR QUALITY 
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1.2.1 ZERCOM 



Subroutine ZERCOM is called from BLDARM to initialize the arm data 
COMMON blocks prior to creating a new arm. The COMMON blocks initial- 
ized to zero include: 


1) BLDGBK - geometric properties^ 

2) BLDMBK - mass properties j 

3) GRAFBK - graphics data. 


SUBROUTINE ZERCOM 


1 

ZERO VRRIRBLES IN GEOMETRIC 
PROPERTIES COMMON BLOCK BLDGBK 

DO UNTIL N = NUMBER OF LINKS 
CURRENT RRM 

IN 

1 

ZERO VRRIRBLES IN MRSS 
PROPERTIES COMMON BLOCK BLDMBK 

DO UNTIL N = NUMBER OF LINKS 
CURRENT RRM 

IN 

ZERO VRRIRBLES IN GRAPHICS 

BLOCK GRRFBK 

COMMON 

RETURN 

END 
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1,2.2 DEFUNIT 


Subroutine DEFUNIT is called during system definition to set up input 
and output units specified by the user. If these I/O units are not met- 
ric, the routine establishes conversion factors between the I/O units 
and internal (metric) units. The conversion factors are stored in vari- 
able CONUNIT, and LISUNIT contains a character string listing the I/O 
units employed. 
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1.2.3 CREATARM 


ORIGINAL r: ■' . 

OF POOR 



Subroutine CREATARM is called within the system definition function to 
provide control of the creation or modification modes for the simple 
cylinder arm data file. The basic routines called for either option are 
SPAN (define arm span), BASE (define base properties), JOINT (define 
joint), ACTUATOR (optional, to define motor properties), LINK (define 
link properties), DEFSPJT (optional, to define special joints), TOOLJT 


(define tool-joint properties) and TOOLLK (define tool-link properties). 
Graphics may be requested during CREATARM. 
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1.2.4 RDARM 


Subroutine RDARM is called from BLDARM to read from an unformatted arm 
data file the data describing a single arm. Routines called by BLDARM 
can then modify the data. The user is prompted for the name of the arm 
data file to be modified. The following data are read from it: 



1) Input/output units; 

2) Geometric properties; 

3) Mass properties; 

4) Graphics data; 

5) Actuator data; 

6) Information on special joints. 

The user has the option of saving or deleting the old data file. 


SUBROUTINE RORRM 


PROMPT 

FOR NRME OF ARM DRTR FILE 






OPEN 

RRM DRTR FILE 




READ 

INPUT/OUTPUT UNITS DRTR 


read geometric properties drtr 




READ 

MRSS 

PROPERTIES 

DRTR 






RERD 

GRAPHICS DRTR 






RERD 

ACTUATOR DRTR 


read special joint drtr 

display 

MESSAGE THAT RI_L_ DRTR HRS BEEN RERD 







SR VE 

RRM DRTR FILE 

/F 

— * rA 



C=D 



\ 

DELETE 

RRM 

4 


Li — 1 
CO 

cxz 

llj 


T \ 


Z 



o 

Lj — 1 

cn 

CO 




CLOSE RND 



c ? 

1 — 1 
Li — 


DELETE FILE 

... , 

SRVE FILE 



RETURN 

r END J 
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1.2.5 BLDDAT 


Subroutine BLDDAT provides the user the capability to specify a more 
detailed physical representation for the links of the robotic arm. 
Components of the robotic arm system are defined by combinations of 
geometric primitives. A number of detailed components can be included 
for the base, each link extension and the tool definitions. The compo- 
nents are simple three-dimensional shapes: the cylinder, cone, rectangu- 
lar solid, symmetric trapezoid, nonsymmetric trapezoid, triangular beam, 
data tablet structure, fillet component, or nonplanar entity. Unique 
subroutines are called to handle loading the graphics object data for 
the shapes chosen to represent a detailed arm. Additional shapes can 
be added as required. 


SUBROUTINE BLDDRT 



ORIGINAL l# 
OF PCCu 
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1.2.6 WRTARM 


Subroutine WRTARM is called from BLDARM to save, in a user-specified 
file, the data generated when creating or modifying an arm description. 
The user is prompted for the name of this file and is also given the op- 
tion of storing a formatted file containing the arm description for 
later printing. 



SUBROUTINE WRTRRM 


PROMPT FOR NAME OF FILE TO WRITE RRM DRTRTO 


OPEN FILE 

WRITE FORMATTEO RRM GEOMETRY FXL-E FOR PRINTING /p 

PROMPT FOR NAME OF RRM GEOMETRY PRINTOUT FILE 

. . 

OPEN FILE 

Zj 

WRITE RRM GEOMETRY DESCRIPTION TO FILE 

z 

~ CLOSE RND SRVE FILE 



WRITE INPUT /OUTPUT UNITS DflTn TO FILE 


WRITE RRM GEOMETRY DRTR 


WRITE RRM MRSS PROPERTIES 


WRITE RRM GRRPHICS DRTR 


WRITE RCTURTOR DRTR 


WRITE SPECIRL UOINT DRTR 


DISPLRV MESSRGE THRT FILE WRS WRITTEN 


DLOSE RND SRVE FILE 
RETURN 
END 
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1.2.7 ENVIR 


Subroutine ENVIR interactively establishes the basic properties of the 
system environment during system definition. This includes the input/ 
output units, the gravity vector and the system span. 


SUBROUTINE ENVIR 


PROMPT 

USER 

FOR GRRVITY FLRG. STANDARD 
OR INPUT VALUE 


RERD USER RESPONSE 



USER REQUESTED STRNDRRD 
GRRVITY 

/f 


s 5 


PROMPT USER FOR INPUT ACCELERATION 
DUE TO GRRVITY IN DEFAULT UNITS 


» § 
& °f 


RERD USER INPUT GRRVITY VECTOR INTO 
GRRV PRRRMETER 


t3 a 
i— cS 

*7 


CALL CVTUNIT GRRV TO CONVERT 
TO INTERNAL MRTH UNITS 


PROMPT 

USER FOR SYSTEM SPAN IN 
DEFAULT UNITS 



RERD USER INPUT INTO SVSSPN 

PRRRMETER 


CRLL CVTUNIT TO CONVERT SVSSPN TO 
INTERNRL MRTH UNITS 


RETURN 


END 
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1.2.8 RDENVS 


The subroutine RDENVS is called from BLDSYS if the user wishes to in- 
clude an environment in the system being created. This routine reads 
the unformatted environment data file created by the system definition 
function for the multiarm system. The user is prompted for the name of 
the data file under which the environment data have been stored. The 
file is opened and COMMON block ENVTBK loaded from the data file during 
system creation. The file is closed and saved. 


SUBROUTINE RDENVS 

PROMPT FOR NAME OF FILE CONTAINING 

ENVIRONMENT D ATA 

OPEN ENVIRONMENT DATA FILE 


REPO INPUT/OUTPUT units data 


read GRAPHICS DATA FOR EACH 

COMPONENT 

DO UNTIL N = NUMBER OF COMPONENTS 
IN ENVIRONMENT 

CLOSE FILE AND SAVE 


DISPLAY MESSRGE Tl— IPT FIL.E HRS BEEN 

RERD RND SAVED 


RETURN 


END 
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1.2.9 RDARMS 


Routine RDARMS is called during the total robotic system creation for 
each of the arms desired for inclusion in the system setup. The sub- 
routine RDARMS reads the unformatted data file created by the system 
definition function containing any one arm file. The user is prompted 
for the name of the data file under which the arm data have been stored. 
The file is opened and read into the following COMMON blocks: GEOMBK, 
AMASBK, IOBJBK, TOOLBK, FORCBK, MOTORBK and SPJTBK. 


SUBROUTINE RDRRMS 


SET PROCESSSOR MODE - 1. FOR SYSTEM DEFINITION 

PROMPT USER FOR FILENAME OF SINGLE ARM FILE TO RERD 

OPEN RRM DRTR FILE 

STORE SYSTEM UNITS IN TEMPORARY ARRAYS 

RERD UNITS COMMON BLOCK INTO SYSTEM COMMON 

RERD GEOMETRY COMMON BLOCK FOR DRSE. JTS. RNO TOOL- INTO SYSTEM ”1 

COMMON 

RERD MRSS PROPERTIES COMMON BLOCK FOR BRSE. JTS, RNO TOOL INTO 

SYSTEM COMMON 

CONVERT PROPERTIES TO INTERNAL UNITS FOR SLIDING RNO ROTRTING 

UTS. 

PUT TOOL MRSS PROPERTIES INTO SYSTEM TOOL COMMON BLOCK 

RERD GRRPHICS DRTR COMMON BLOCK INTO SYSTEM COMMON 

CONVERT RRM SPAN TO INTERNAL UNITS 

SCALE GRAPHICS OBJECT DRTR BY (ARM SPAN / SYSTEM SPAN) 

RERD RCTURTOR DRTR COMMON BLOCK INTO SYSTEM COMMON 

CONVERT RCTURTOR DRTR TO INTERNRL UNITS 

RERD SPECIRL JOINT DRTR COMMON BLOCK INTO SYSTEM COMMON 

CLOSE SINGLE RRM DRTR FILE 

REWRITE SYSTEM UNITS INTO SYSTEM UNITS COMMON BLOCK 

RETURN 

END 


original 

poor 


Quality 
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1.2.10 BASES 


BASES modifies the base location or orientation when including an arm 
in a system; it is called from BLDSYS. 


SUBROUTINE BASES 


SET MODE FI_RG — 1 

write current brse locrtion value to terminal 

PROMPT FOR BASE LOCATION MODIFICATION FL FG „ 


INPUT MODIFY FI_RG — 2 



fwui.^t toa new x» y, m looi t o f awe tn wowlo UXJWDa CMLJI 1 3 

PERU 

U1RI] 

-TE CURRENT BRSE ORIENTION VALUE TO TERMINAL 


1 

1 

1 PROMPT FOR BRSE ORIENTATION MODIFICATION FLRG _ 


INPUT MODIFY FLAG - 2 



I - o 



DO WHILE I . LT. 3 




I -» X -h 1 




PROMPT FOR I TH ROTATION SEQUENCE AXIS OF ROTATION 




RERD ROTATION SEQUENCE RXIS OF ROTATION INTO IROT CID 




PROMPT FOR X TH ROTATION PRINGLE 




RERD ROTRTXON ANGLE INTO PJTRNO FOR I TH SEQUENCE NUMBER 




PROMPT FOR USER INPUT TERMINATION 



DO 

UNTIL USER TERMINATES ROTATION SEQUENCE INPUT 


SET JNT3CQ MATRIX CLEMENT BREED UPON XROT MRTRIX CLEMENTS __ 

LOAD TEMP MATRIX WITH CURRENT ORIENTATION MATRIX 


RQTMRT TO COMPUTE ROTATION MATRIX* TEMPI „ — . 

CONCATENATE NEW ROT MRTRIX {TEMPI ) WITH CURRENT ROT MRTRIX (TEMPJ 


Lord rotation matrix. 'or. with resultins transformed matrix 

— RETURN 

END 



ORIGINAL PAGE fiS 
OF POOR QUALITY 
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1.2.11 SETUP 


Subroutine SETUP calls SETUP2 for each arm in the manipulator system to 
calculate the positions of all arm components in terms of world coordi- 
nates. 


SUBROUTINE SETUP 


CRI_L_ SETUPS TO CRLCULRTE ALL 
POSITIONS IN WORLD COORDINRTES 


DO UNTIL KRRM = NUMBER OF RRMS IN 
THE SYSTEM 


RETURN 


END 
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1.2.12 SETUP2 


SETUP2 works every increment. It calculates the positions of all links 
(including base, tool, and any held loads) and transforms link and cen- 
troid vectors to world coordinates. The recursive positioning method 
described in the main text is used. Finally, subroutine JACOB is called 
to compute the Jacobian for the current position. 



SUBROUTINE SETUP2 


CflLL BflSRJT TO LORO SfiSE LOC. RNO ORIENTATION INTO PROPEER 

VARIABLES 

CflLL MflTMPY TO FIND JOINT N TO JOINT N-*- 1 VECTOR 

CHIJJ 



CALL MRTMPY TO FIND JOINT N TO LINK N 

VECTOR CHCG) 

C. G. 


T\ 

N . LT. 

NUMBER OF JOINTS -*• 1 (NOT AT 
EFFECTOR3 

END 

/f 

t\ 

JOINT 



IS HINGE OR SNIVEL 




CALL CETM RNO MRTMPY TO 
GET JOINT N TO N-l 
TRANS. MATRIX (RJL) 


FOR SLIDING JOINT SET 
RJL - DATA IN ARRAY OR 


CflLL MRTMPY TO GET JOINT N TO 
WORLD TRRNSFORMRTION MRTRIX 


UPDATE HIU FOR SLIDING JOINTS 


UPDATE POSITION VARIABLE POS 


DO UNTIL N = NUMBER OF JOINTS 


CflLL 

MRTMPY TO GET HCG FOR END 

EFFECTOR 


CAUL. MRTMPY TO FXND 

LOCATION OP END EFF. REF. PT. 

XN WORLD COOR. 


^ 

FIRM 

IS HOLDING R LORD OBJECT 

si 

UPDATE LOCATION 

OF HELD LOAD OBJECT 


CNULL3 

CflLL MflTMPY 

TO UPDATE ORIENTATION OF HELD LOAD 


CRLL 

URCOB 

TO CRLCULRTE THE 

JACOBIAN 


RETURN 

END 
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(NULL) 









1.2.13 HARD COP 


Subroutine HARD_COP is executed when a hardcopy record of the current 
Evans and Sutherland display may be desired. This routine queries the 
user to determine if a hardcopy is desired and runs the appropriate rou- 
tines to create a picture file for later translation into a hardcopy 
plot. 


(VCLR for HARD COP is not available.) 
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1.2.14 SYSGRAF 


Subroutine SYSGRAF provides the system definition graphics capability 
in the system definition function. SYSGRAF displays the environment, 
load and robotic arm choices for building a robotic system scenario. 

It takes as input through the calling sequence, the number of arms in 
the system, a flag indicating the existence of an environment file for 
the system and a load objects file inclusion indicator. It uses the 
system span input by the user to scale the graphics picture. IFLAG con- 
trols the logical flow in the subroutine. If IFLAG S 1, the graphics sys- 
tem is initialized and displayed in the initial condition; if IFLAG *2, 
the robotic system, loads and environment are displayed; if IFLAG 3 3 , the 
graphics display is terminated. In the update mode, the environment is 
constant and therefore not updated. As before, the Evans and Sutherland 
graphics routines are used to provide all graphic capabilities. 


SUBROUTINE SYSGRRF 

“set PROCESSOR MODE ■ 1 ~. FOR SYSTEM DEFINITION 


NOT INXTIflLlliNO QHOPHICS QISPLfiY . ] 

i-m ranj TO •TWTUR WEM.CW COUTWOU OXMU* 

*«t XNTca«R iim »m i.r i'i'm m*a mjrmnan 

CNULL5 


no FOR EACH RAM IN SYSTEM . .. . 

INITIALIZING DISPLAY AND DBAUING FIRST RAM 

^ — r 

^ T . i^jj u. i' tq 1RH p. „ 

(NULL] 

«r r Lm avrmjT tmmi mho memommm mrr am mmtmrrm 


Htti rrr him™ mm — «n a«.i im mw 

"V- UPDATING DISPLAY 

^3 

^ FOR F±RST AAM. CRLL DRTOUT TO OUTPUT DISPLAY SET UP _.__J 


ENVIRONMENT DATA EXISTS AND DRAWING FIRST ARM 


" 9CT hU4MN OW COTCMNTI XN KMVXF. I'WW«T«W . . . _ 1 

(NULL] 

— — < nrrr wnv. mmmv foi mwio Furai 


CMJL 0»T. TO OUIW , JXY iWXS» #«NT 

swt row l-wt bnv. omumerr mmmn- uPONTxon ummn 

LOAD DATA EXISTS 


'□Q ro« EWCH LORO XN LORO 


BJECTS FILfi 


SET TRffNSP’ORMRTXON Off SCO ON LOC. ~RNO ORXgNTRTION OF UQQD 


SET N UMag R O F COMPONENTS XN LOflO PflWnne. I g R 

XJnT 


UORD LORD OPU. PRRRV FOR SRQPMXCS Ft-RQS 


RTR TO OXSPt_R 


SET COUNTER EOF LAST CORO OflJgCT 


RV LOCHTXON USED 


RT LEAST 


ROBOTIC ARM EXISTS 


T ^ 

SET TRANSFORMATION BASED ON POS. AND ROT. OF BASEA.INK/TOOL ^ 

i i 


SET NUMBER OF COMPONENTS XN BASE/LINK/TOOL PRRM j 



DO FOR EACH COMPONENT XN BRSE/LINK/TOOL 


— 1 : ! 


load SVS PPM OBJ. ARRAY FOR GRAPHICS FLAGS 


=5 i ; 


CRI_I_ D3DPTR TO DISPLAY ARM 




SET COUNTER FOR LAST BASE/LINK/TOOL OBJ. ARRAY LOG. USED 


i ||| 

□ o 

UNTIL BASE. ALL LINKS AND TOOL HAVE BEEN QRPWN 


i -i j { 

1 

CLOSE AND REPLACE SEGMENT 


i 


IT s. 1 1] 

CNULLJ 


ORiG’NAL PAGE iS 
OF POOR QUALITY 
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1.2.15 RDLODS 


RDLODS reads in load data if the user requests that loads be included 
in the robotic system under construction. The file read is the unfor- 
matted load file created by the system definition function. Subroutine 
RDLODS prompts the user for the name of the file containing load data, 
and then reads those data into COMMON blocks LGRAFBK and LOADBK during 
system creation. The load file is scaled from load units to internal 
system units, closed, and saved. 


SUBROUTINE RDLODS 


PROMPT FOR NRME OF FILE CONTRINING LORDS 1 

□RTR 

LORDS DATA FILE EXISTS ^/p 

OPEN LORDS DRTR FILE 


STORE SYSTEM I/O UNITS IN DUMMY 

VARIABLES 

READ LORD OBJECTS INPUT/OUTPUT UNITS 

□ RTR 

READ LORD MRSS PROPERTIES DATA 

\ LORD OBJECTS I/O UNITS NOT / 

T\ METRIC / F 


CONVERT LORO VRRXRBLES TO fMt 11 1 1 

INTERNAL CMETRXC) UNITS UNi_il_L_j 


RERD LORD OBUECTS GRRPHICS DRTR 

CLOSE RND SR VE FILE 

RESTORE SYSTEM I/O UNITS 

i 

RETURN 

END 
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1.2.16 LOCMOD 


Subroutine LOCMOD is called from BLDSYS to allow the user to modify the 
locations and orientations of load objects when building a system. The 
current location is displayed and then the user is prompted for a new 
location. The subroutine also displays the transformation matrix for 
the current orientation of the load object and prompts the user for a 
sequence of rotation axes and angles that define a change in orienta- 
tion. ROTMAT is called to calculate the transformation matrix from the 
user input, and MATMPY combines this new transformation matrix with the 
old one. 



SUBROUTINE LOCMOD 

DISPLAY 

CURRENT LOCATION OF LORD 
OBUECT 


PROMPT 

USER FOR AND RERD IN NEW 
LORD OBUECT LOCRTION 


DISPLAY 

CURRENT 

TRANSFORMATION MRTRIX FOR 
ORIENTATION OF LORD OBUECT 


X 

MODIFY ORIENTATION 
— 1 



DC 

RC 

USER INPUT OF A ROTATION 

AXIS 


USER INPUT OF ROTATION 

ANGLE 

UNTIL USER STOP FLAG OR 3 
1TATIONS 

! 

Z 

CALL ROTMAT TO CALCULATE NEW 
TRANSFORMATION MATRIX 

CALL MATMPY TO COMBINE OLD AND 
NEW TRANSFORMATION MATRICES 

RETURN 

END 
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1.2.17 WRTSYS 


The subroutine WRTSYS writes the unformatted data file created by the 
system definition function containing the contents of pertinent COMMON 
blocks defining the robotic system: GEOMBK, AMASBK, IOBJBK, ENVTBK, 
LOADBK, TOOLBK, UNITBK, FORCBK, MOTORBK, SPJTBK and LGRAFBL. 


SUBROUTINE WRTSYS 























1.2.18 RDENV 


The subroutine RDENV reads an unformatted environment data file during 
the system definition function. The content of the file is the perti- 
nent COMMON block defining an environment for the robotic system. The 
user is prompted for the file name from which the file is to be read. 



PROMPT USER FOR FILENAME OF 
ENVIRONMENT FILE TO READ 


OPEN ENVIRONMENT DATA FILE 


RERD UNITS COMMON BLOCK 


READ environment graphics drtr 

COMMON BLOCK 


CLOSE ENVIRONMENT DRTR FILE 


RETURN 


END 
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1.2.19 DRWENV 



DRWENV is called within the system definition function from BLDENV to 
provide graphics display during the generation of detailed environment 
graphics representations. It is called to display each successive en- 
vironment component as it is defined. 


SUBROUTINE DRUJENV 


SET PROCESSOR MODE - 1. FOR 

SYSTEM DEFINITION 


— n 

|>\ INITIALIZING 


Kl 

PROMPT USER FOR ENVIRONMENT SPRN 

(NULL) 



SET SCRLE FACTOR# IFACT - 

lOOO. /ENVIR. SRRN 



INITIALIZING 

DISPLAY 



SET WINDOW BOUNDARIES 

OLL OXPLJB TO STOTUB RNFCOG CONTROL OXf*.8 

DRAWING OR REPLACI 

NG COMPONENT 


SET WINDOW BOUNDARIES 


SET CURRENT INTEGER TRANSLATION AND ROT. VALUES 


SET NUMBER OF COMPONENTS IN ENVIR. PRRRMETER 


DO FOR RLL COMPONENTS BEFORE CURRENT COMP. 



SET GRAPHICS FLAGS FOR SEQUENTIAL AND 

ALTERNATING PTS. 


. — . 

CALL D3DRTR TO DISPLAY 

COMPONENT 

1 

— ! 

SET COUNTER FOR LAST ENV. OBJECT ARRAY LOCATION USED 

1 

=i 

SET GRAPHICS FLAGS FOR SEQUENTIAL AND ALTERNATING PTS. 


CALL D3DATA TO DISPLAY COMPONENT 


SET COUNTER FOR LAST ENV. OBJECT ARRAY LOCATION USED 


CLOSE AND REPLACE SEGMENT 


RETURN 

END 
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1.2.20 WRTENV 


Subroutine WRTENV writes an unformatted environment data file during 
the system definition function. The content of the file is the per- 
tinent COMMON block defining an environment. 



SUBROUTINE WRTENV 


PROMPT user for filename of 

ENVIRONMENT FILE TO WRITE 


OPEN ENVIRONMENT DATA FILE 


WRITE UNITS COMMON BLOCK 


WRITE ENVIRONMENT GRAPHICS DATA 

COMMON BLOCK 


CLOSE ENVIRONMENT DATA FILE 


RETURN 


END 




B-46 


1.2.21 RDLOAD 


The subroutine RDLOAD reads an unformatted load objects data file during 
the system definition function. The contents of the file are the per- 
tinent COMMON blocks defining a load file for the robotic system. The 
user is prompted for the file name from which the file is to be read. 


SUBROUTINE RDLOAD 


PROMPT USER FOR FILENAME OF LORD 
OBJECTS FILE TO READ 


OPEN LORD OBJECTS DRTR FILE 


RERD UNITS COMMON BLOCK 


READ LORD OBJECTS MRSS PROPERTIES 

COMMON BLOCK 


RERD LORD OBJECTS GRRPHICS DRTR 

COMMON BLOCK 


CLOSE LORD OBJECTS DRTR FILE 


RETURN 


END 











1.2.22 LOAD 


Subroutine LOAD is called during the BLDLOD option of INITDRVR. It al 
lows the user to create and define the mass properties of one or more 
load objects. If a file of load object data already exists, this sub- 
routine may be used to modify portions of those data. The load para- 
meters for which the user is prompted are listed: 

1) Location and orientation with respect to the world coordinate 
system; 

2) Length and radius; 

3) Center of mass; 

4) Mass; 

5) Inertia distribution; 

6) Mass and location of any point masses included. 


SUBROUTINE LORD 


MODIFYING OI_D LORD OBJECT MRSS PROPERTIES 

I PROMPT USER FOR MODXFXCRTXON CATEGORY 

PROMPT use* FOP LOAD OBJECT LOCATION 
COOROlNnTGS 

UJ 

in 

a 

u 

0 

□ 


PROMPT USER FOR ROTATION RXXS 

PROMPT USER FOR ROTATION RNQL-E 


CRT - 
ll. LOC 
AND OR 



DO UNTIL- USER STOP OB 3 ROTATIONS 


COLL ROTMRT TO CRLCULRTE 
TRRNSFORMRTXON MRTRXX 

PROMPT USER FOR LENGTH OF 
LORD OBJECT 

PROMPT USER FOR RADIUS OF 
LOAD OBJECT 


PROGRRM FXNDS 
CENTER OF MRSS 

/f 

PUT CENTER OF 
MASS AT 3EO«TAIC 
CENTER 

PROMPT USER FOR | 

CENTER OF MRSS i 

COORDINATES 

PROMPT USER FOR MRSS OF 
LORD OBJECT 

IX 

PROGRAM COMPUTES 
INERTIR MRTRXX 

A 

CALCULATE INERTIA 
MATRIX 

PROMPT USER TOR 
COLUMNS OF 
INERTIR MRTRXX 

X 

INCLUDE POXNT 
MRSSES 



i PROMPT FOR MRSS 
OF P OXNT MRSS 

PROMPT FOR LOCRTIONOF 
POINT MASS 


UKlGINAL PAGE IS 

OF POOR QUALITY 
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1.2*23 TLDMAS 


Subroutine TLDMAS is called from BLDLOD to add the effects of point 
masses included in a load object. Variables for total mass, centroid 
location, and rotary inertia are initialized with the values for the 
simple load object. If point masses are included, ADDMAS is called to 
calculate new values for these variables that include the point mass 
effects. 
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1.2.24 DRWLOD 


DRWLOD is called within the system definition function from BLDLOD to 
provide graphics display during the generation of a detailed load ob- 
jects file. It is called to display each successive load object compo- 
nent as it is defined. 


SET 

PROCESSOR 

wnnE - 1. FOR SYSTEM DEFINITION 

INITIALIZING 





tV 

2EPO INTEGER 




TRRNSLRTION RND 
ROTATION VRLUC3 

DRflWIN 

LW1L.L. t w • — ... 

SET CURRENT INTEGER TRANSLATION AND ROT. VALUES 

G HR REPLACING COMPONENT 


1 ^ 

SET 

TRANSFORMATION BASED ON LOG. RND ORIENTATION OF LOAD 

. 


SET WINDOW BOUNDARIES 


SET 

CURRENT INTEGER TRANSLATION RND ROT. VALUES 



j 

tDRRU COL- OR COORDINATED RXES SYSTEM CRED-X, WHIT-Y, BLU S 


SET 

NUMBER OF 

COMPONENTS XN LOAD PARAMETER 



DO 

FOR ALL COMPONENTS BEFORE CURRENT COMP. 



SET 

QpppHICS FLRG3 FOR SEQUENTIAL HNP re.niNt-1 1 f- ~ . 

r-oi L naORTR TO DISPLAY COMPONENT 



jcap-r r*oi 1 NITBER FOR LAST LOAD OBJECT PRRRY LOCATION USED 



SET 

QpppHICS FLAGS FOR SEQUENTIAL RND ALTERNATING RTS. 



Cni_l_ D3DHTR TO DISPLAY COMPONENT 


SET 

COUNTER FOR LAST LORD OBJECT RRRRY LOCATION USED 




CLOSE 

RND REPLACE SEGMENT 

RETURN 

END 




ORIG^flL PAGE & 
OF POOR QUALITY 
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1.2.25 WRTLOD 


Subroutine WRTLOD writes the unformatted load objects data file during 
the system definition function. The contents of the file are the per- 
tinent COMMON blocks defining the mass properties and graphics of the 
loads for a system. 


SUBROUTINE WRTLOD 


PROMPT USER FOR FILENAME OF LOAD 
OBJECTS FILE TO WRITE 


OPEN LOAD OBJECTS DATA FILE 


WRITE UNITS COMMON BLOCK 


WRITE LORD OBUECTS MRSS PROPERTIES 

COMMON BLOCK 


WRITE LORD OBUECTS GRAPHICS DRTR 

COMMON BLOCK 


CLOSE LORD OBUECTS DRTR FILE 


RETURN 


END 
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1.3.1 GRINIT 

For building an environment, simple arm, detailed arm, or loads with 
graphics, routine GRINIT initializes the E&S display, extended switches/ 
lights, and analog control dials, and draws the graphics display border. 
Working from the input flag IFLAG, the type heading of the general dis- 
play is chosen, either simple cylinder, detailed geometry, environment 
or load. The graphics segments are opened and the title for the system 
definition function driver currently under execution is output. 


SUBROUTI NE GRINIT 

CALL MPINIT TO INITIRLIZE GRAPHICS 

SYSTEM 

INITIALIZE EXTENDED FUNCTION KEY 

SNITCHES 

INITIRLIZE EXTENDED FUNCTION KEY 

LIGHTS 


INITIRLIZE RNRLOG CONTROL DIRLS 


INITIRLIZE EVENT QUEUE 


DRAW GRRPHICS DISPLRY BORDER 


OUTPUT GRRPHICS DISPLRY TITLE 


I OUTPUT MRRTIN MRRIETTR COMPRNY LOGO 

| TRANSFER INITIAL RERDINGS OF 

I CONTROL DIRLS FROM DEVICE QUEUE 


END 
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1 . 3.2 


The manipulator arm span is requested as input from the user during ini 
tial creation of simple cylinder arm data; modification of the ARMSPN 
value is also allowed through a call to SPAN during the CREATARM modifi 
cation mode. 


SUBROUTINE SPRN 


ERROR CODE = O 


SYSTEM MODIFICATION FLAG IS 

SET 


PROMPT FOR SPRN 
VALUE MODIFICATION 
FLAG 


CNULL) 


INPUT MODIFY FLAG . NE. 1 

PROMPT FOR NEW SPRN. 

OR RERCH. OF RRM 

RERD SPRN INTO RRMSPN 
PRRRMETER 


CNULL) 


RETURN 



1.3.3 BASE 



PROMPT USER FOR EACH BftSE PRRRMETER VALUE 









1.3.4 OBJECT 


Subroutine OBJECT creates simple cylinder graphics data used by the gra- 
phics package to draw the robotic arm during the system definition func- 
tion. The data created in OBJECT are stored in COMMON block IARMOBJ, 
and represent a right circular cylinder of the specified size for each 
system component (the base, each link and the tool). It is called for 
generation of each of these components in turn. 


SUBROUTINE OBJECT 


SET ERROR CODE FL.RG - 


SCALING FACTOR* IFflCT . - lOOO- /ARM SPRN VALUE 




SET X = ENDPTS SCALED BY IFACT 


DO FOR EACH VERTEX OF* END CIRCLE OF CYLINDER 


'COMPUTE Y VALUE OF POINT 08 ROOZUS«*COS tRNGLE 
3U8TOOED3 


COMPUTE Z VALUE OF POINT RS RRQXU9*SIN (ANGLE 


LOAD IPPMOBLL 8LCMENTS XCNTWW-I^l — ► ICNTAWM+B. 

MXTH X. V. m VHLUgl 

LORO IARMOBJ ELEMENTS FOR ORSE SIDES CONNECTING 
ALTERNATING POINTS 


RETURN 


END 


LORD IARMOBJ 
CYLINDER SIDE 
ELMTS. rOR 
CONNECTING 
ALTERNATING 
POINTS 


INCREMENT 
ICNTRRM AND 
INDEX VALUE FOR 
LOC. OF NEXT 
RRRRV COMPONENT 
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1 . 3.5 


GRAPH 


If the graphics are initiated during the simple single arm creation or 
modification, subroutine GRAPH provides the graphics capability for the 
simple cylinder representation of the robotic manipulator. GRAPH dis- 
plays each base, joint/link combination and tool as they are defined. 
Graphics during the modification mode is handled with calling arguments 
input to GRAPH; appropriate deletions, additions and changes to the 
links are visually depicted. GRAPH provides only the simplified robotic 
arm definition display. 



CRt_i_ TTRRN for picture processor transformation matrix 


EXTRACT OFFSET JT. RNG. FROM JOINT VARIABLE RRRRY 


I I or r w)L. I w ■ - ’ " - . — 

Cni-L. TROTX OR -V UITH _JT. RNG, TO ROTATE TRANSFORMATION 


EXTRACT ROTATION AXES USED XN ORXENTINS* FROM .JQXNT SEQUENCE RRRRY 


EXTRACT X. Y« g ROT, RNGL.ES USED XN ORIENTING* PROM JT. RNGL.E 


CALL TROTX. —V OR -Z UIITH INTEGER RNG, TO ROTRTE TRANSFORMATION 


SET NUMBER OP COMPONENTS XN LINK PARAMETER^ 


DO FOR EACH COMPONENT XN LINK 


SET STRRT LOCRTIDN IN ARM OBJECT ARRAY FOR CURRENT COMPONENT^ 
LOAD ARM OBJ. RRRRY FOR SRAPHICS FLAGS 


LOAD RRM OBJ. RRRRY FOR SEQUENTIAL POXNTS" 


LOAD ARM OBJ. RRRRY FOR RLTERNATING POXNTS 


CALL D3QRTR TO DISPLAY LINK 


r~ni IMTFP FOR LAST RRM OB JECT RRRRY LOCATION USEZ^ 
EEN DRAWN 


DO UNTIL RUL. EXXSTXNQ LINKS HAVE 



ORSG!NAL PAGE IS 
OF POOR QUALITY 
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1.3.6 DIALS 


DIALS is called to scale the Evans and Sutherland analog control dials 
values read during camera perspective changes via the extended E&S 
dials. The values are scaled to integers between -32767 and +32767. 


SUBROUTINE DIRLS 


TRANSFER GRAPHICS RNRLOG CONTROL 
DIALS DEVICE RECORD FROM QUEUE 


DO FOR EACH CONTROL DIAL VALUE READ 


VALUE . GT. 32767. 

T \ /> 

f 

SET VALUE TO 
-65534 ■+* VALUE 

CNULLD 

x 7 

\ VALUE . LT. -32767. / 

_i\ 

SET VALUE TO 
65534 ■+■ VALUE 

(NULL) 


RETURN 


END 
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1.3.7 JOINT 


For JOINT, the user inputs joint type, joint location as Cartesian coor- 
dinates in terms of the coordinate system of the previous joint (or 
base, if the current joint is joint 1), joint orientation as a rotation 
sequence of axes and corresponding angles with respect to the previous 
joint coordinate system (or base, if joint 1), and initial joint state 
(initial angle for hinge or swivel, or initial length for sliding 
joints). The x-axis of a joint coordinate system is directed along the 
centerline of the link between joint i and joint i+1 (or end-effector 
if the current joint is the final joint in the system). JOINT is called 
by CREATARM during initial creation or modification of arm data. 


SUBROUTINE JOINT 


PLAO — 


SET ERROR COPE PT-FQ~~- O 


ggRO R OTATION SEQUENCE^ MATRIX* 
~ INPUT MODIFY FLRQ — 1 


TROT 


PROMPT ~ POR JOIN T MOOirXCRTXON DCjiggP 


loo CRag" ON MOOXFT CRTEQORY 


i I wr-* ^ ■ 1 

j gRp MOOXPXCRTXON crtesorv desired 


OOCflSE 


JOINT 


H 

tel 


s| 

*s 

55 


ROTATION SEQUENCE AND 
ROTATION RNSLES 


IR == O 


s 

a - 


DO WHILE 
L.T. 3 


XR 


IR - IR 


PROMPT FOR XR TM 
ROTATION SCQUCNCC 
AXIS OP ROTATION 


— ssss: sss-s&sr 1 


PROMPT FOR XR TH 
ROTATION RNGL.E 


DO UNTIL USER TERMINATES 
ROTATION SEQUENCE INPUT 


RERD ROTATION ANGLE 

into rjtang for XR 

TH SEQUENCE MUMPER 


PROMPT FOR USER 
INPUT TERMINATION 


^ S 

iii 

s i 


s “* s? 


KX**» /MAC LIMIT* 


PROMPT FOR 
TRAVEL 
MIN. /MAX. 
LIMITS 


LIMIT VALUES 
into TAVLMT POR 
JOINT X 



CONVERT LIMIT 

angles from 

DEGREES TO 
RADIANS 


PROMPT FOR 
RATE MAX. 
LIMIT 


iCALL ROTMAT TO COMPUTE 
ROTATION MATRIX. OR 


s a 


JnTXL. MODIF ICATION MOPE DgSIREO TO T ERMINATE JOINT MOPIF ^ 


READ RATE 
MAX. LIMIT 
INTO RATLMT 
FOR JOINT I 
SET RATE 
MIN. LIMIT 
i - - RRTE 

i MAX. LIMIT 


ORIGINAL HIC'H 13 ' 
OF POOR QUAL!TY 
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PROMPT USER FOR ERCH JOINT PflftftHETER VALUE 






1.3.8 ACTUATOR 


ORIGINAL pp'-:~ 

OF POOR QiJAU 


ACTUATOR allows Che user Co define or modify Che COMMON blocks defining 
acCuaCor propercies for Che arm by inCeracCively prompCing for acCuaCor 
pararaeCer values or by reading a previously consCrucCed file of acCuaCor 
paramecer values. The user can opC for no acCuaCor definicion if 
desired. 


SUBROUTINE RCTURTOR 
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1.3.9 LINK 


In the create mode of LINK, the user is prompted for link endpoints in 
coordinates along the x-axis, link radius, the location of center of 
mass as the Cartesian coordinates of the center of gravity in the coor- 
dinate system of the joint at the "base" end of the link, link mass and 
inertia matrix relative to the centroid and the number of sides for the 
desired simple cylinder. 


SUBROUTINE LIN K 

- OCT x"~* 1 

' H o cayy fuwp - i 

' Rg* o MomcFXCwfxdrT ewTgo6wY"~i^«g^p_ __ 



POINT MR33 VPLLlES PND 
LOCH T ION3 Bt-ONG LINK 

SET POINT MASS 
COUNTER. U - O 










1.3.10 DEFSPJT 


This routine interactively establishes the number, type and location of 
"special joints." These are joints for which a constraint is placed on 
the relative joint displacements. 


SUBROUTINE DEFSPUT 


PROMPT FOR NSPUT — THE NUMBER 
OF SPECIAL JOINTS IN THE ARM 

□O FOR ERCH SPECIAL JOINT 


PROMPT FOR TYPE OF SPECIRU 
JOINT AND SET ISPTYP 

PROMPT FOR WHICH JOINT OF 
RRM THE SPECIRU JOINT IS RND 

SET NJTSP 
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1.3.11 TOOLJT 


Subroutine TOOLJT is called by CREATARM to interactively define or modi- 
fy the geometry properties of the manipulator end-effector. The data for 
which the user is prompted include: 

1) Location of tool with respect to final link; 

2) Orientation of tool with respect to final link. 

The orientation data are input as a sequence of rotations about coordi- 
nate axes and ROTMAT is called to compute the corresponding rotation 
matrix. 


SUBROUTINE TOQLJT 


INITIRLIZE ROTATION SEQUENCE CIROT3 
TO ZERO INDICATING NO ROTRTIONS 


MODIFYING PREVIOUS DATA 


CD CD 
CD Lu_ 


I CO 

an. z 

CD CD 


SUCCESSIVELY PROMPT FOR X, V. RND 
2 LOCATION OF TOOL. WITH RESPECT TO 
PRECEDING LINK 


CD CD CD 
I I CJZ 

CZ CO (DC. 

CD I CD 

I I ZD CD 

U_ CL UJ 


IR 

= IR-*- 1 

ENTER IROT CIR3 

— COORDINATE AXIS 

FOR 

ROTRTION 

ENTER RUTRNG 

CIR3 — RNGLE OF 

ROTRTION 

RBOUT 

THIS RXIS 

1 

PROMPT FOR USER TERMINATION 


CD CO Li_J 


□O UNTIL USER TERMINPTES OR IR « 3 

SET JNTSEQ — IROT C 1 3 IROT C23 IROT C33 

CRLL ROTMAT TO COMPUTE ROTATION MATRIX 
CORRESPONDING TO THIS ROTATION SEQUENCE 



fifPRESENTRIIONI 


1.3.12 TOOLLK 



Subroutine TOOLLK is called by CREATARM to interactively define or modi- 
fy the mass and graphics properties of the manipulator end-effector. 

The data for which the user is prompted include: 

1) Endpoints for cylinder representation; 

2) Radius of cylinder; 

3) Number of sides of cylinder; 

4) Center of mass of end-effector; 

5) Mass; 

6) Inertia distribution; 

7) Location and mass of point masses. 

When modifying existing data, the user has the option of which cate- 
gories to modify. 


SUBROUTINE TOOLLK 

^TmODIFYING old end effect or mrss propertie s 

PROMPT FOR MODIFICATION CATEGORY | PROMPT USER POB ' 


PROMPT USER FOP THE TOOL. END 
POINT COORDINATES OF TOOL 


□OCRSE 



S i * 


1 CO \ 
CO 

F CH T \ 


prompt user for tool RRCIUS CFC3« 
SRRPHICS REPRESCNTflTXON) 


PROMPT USER FOR NUMBER OF SIDES 
OF TOOL CFOR CRRPHICS3 

PROGRAM FINDS 
CENTER OF MRSS 


t * 


RUT CENTER OF 
MRSS AT 

Geometric center 

PROMPT USER FOR 
CENTER OF MRSS 
COORDINATES 

j 


1 1 


PROMPT USER FOR 
MRSS OF TOOL 


PROGRRM COMPUTES 
INERTIA MATRIX 


PROMPT USER FOR 
i COLUMNS OF 

1 INERTIA MATRIX 


INCLUDE 
NT MASSE 

DMPT FOR 
OF POINT 

MRSS 

DMPT FOR 
flTION OF 
CNT MRSS 




STOP 


(NULL) 









1.3.13 TOTMAS 


Routine TOTMAS combines individual components of each link's contribu- 
tions (e.g., link mass, point masses) to obtain a total mass distribu- 
tion for the joint/link combinations and tool during robot arm creation. 
Variables for the total mass, centroid location, and inertia distribu- 
tion are initialized with the values from the simple link. If point 
masses are included, ADDMAS is called to add the effects of these addi- 
tional terms. 


INITIALIZE TOTRL MRSS. C. G. » RND 
INERTIA VARIABLES WITH LINK DATA 

_ — ■ — 1 — 7 

X 

NUMBER OF POINT MASSES 

T 

X FOR CURRENT LINK . GT. 

° X F - 


CALL ADDMAS TO 



INCORPORATE POINT 



MASS DATA 

I J 

DC 

UNTIL IP = NUMBER 

H 

OF POINT MASSES FOR 


CURRENT LINK 



DO UNTIL N = NUMBER OF LINKS IN RRM 
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1.3.14 ADDMAS 


Subroutine ADDMAS combines the mass properties of two objects to obtain 
composite values for the mass, centroid location, and inertia distribu- 
tion. ADDMAS calls ADDMAS2 to perform the computations. ADDMAS then 
loads the results into the first object's mass property variables. 


SUBROUTINE ADDMAS 

CRLL RDOMRS2 FOR COMPOSITE INERTIA 
AND PUT IN TEMP VARIABLES 


PUT MPSS RESULTS INTO MPSS OF BODY 

1 


PUT C. G. RESULTS INTO C. G. OF BODY 

1 


PUT INERTIA MATRIX INTO INERTIA 
MATRIX OF BODY 1 


RETURN 


END 








1.3.15 ADDMAS2 


ADDMAS2 calculates the composite mass properties of two rigid bodies 
joined together. The mass, centroid location, and inertia matrix for 
the composite body are returned as results. 



SUBROUTINE RDDMRS2 


TOTAL MASS 


MRSS OF BODY 1 ■+■ MRSS 

OF BODY 2 


COMPOSITE CENTROID = CMRSS 1 CG 

1 -+■ MRSS 2 * CG 2) / TOTRL MRSS 


R 1 = CG 1 - COMPOSITE CENTROID 


R2 = CG 2 - COMPOSITE CENTROID 


CRLL RCICR FOR R1 SQURRED MRTRIX CR 1 SOD 
USED TO FIND COMPOSITE INERTIA 


CALL RCICR FOR R2 SQURRED MRTRIX (R2SQ) 
USED TO FIND COMPOSITE INERTIA 


COMPOSITE INERTIA = RIN 1 -+• RIN2 

CMRSS 1 R1SQ ■+■ MRSS 2 R2SQD 

RETURN 


END 
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1.3.16 RCICR 


Subroutine RCICR is called by ADDMAS2 to set up the inertia matrix 
corresponding to a point mass displaced from the body centroid. This, 
inertia matrix forms one component of the inertia distribution for the 
composite body. 
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1.3.17 GRTERM 


GRTERM is called to terminate the Evans and Sutherland device processor 
display unit. It calls MPSTOP to terminate the multi-picture processor 
display unit graphics. 


SUBROUTINE GRTERM 


CALL MPSTOP TO TERMINATE GRAPHICS 

SYSTEM 


RETURN 


END 



1.3.18 BASPUT 


Subroutine BASPUT is called from subroutine SETUP2 during position 
calculations for the manipulator. This subroutine takes the position 
and orientation of the base of each arm (with respect to the world 
coordinate system) and loads these data into the arrays POS and ROT. 


SUBROUTINE BASPUT 


1 

PUT ARM 

BASE LOCATION IN ARRAY 
POS 

1 


PUT ARM BASE ORIENTATION IN 

ARRAY ROT 


□ O UNTIL. 

U =** 3 

DO UNTIL I 

- 3 


DO UNTIL. KARM = NUMBER OF ARMS 


RETURN 


END 











1.3.19 JACOB 


Subroutine JACOB sets up the Jacobian matrix that will later be used to 
solve for individual joint velocities for each arm given the end effec- 
tor velocity. This subroutine uses end-effector position and joint to 
world transformation matrices to determine the entries of the Jacobian 
as described in a previous section. The result is a 6xN matrix for each 
arm. 



SUBROUTINE URCOB 


CPLL 

MRTMPV 

TO PUT REF POINT VECTOR 

XN 

WORLD COORDXNRTES 

ODD 

VECTOR 

FROM 

WORLD ORIGIN TO 

ENP 

EFFECTOR ORIGIN 

[ 


FIND 

UOINT RXIS 

OF 

ROTATION 


UOINT IS HINGE OR SWIVEL 


FIND DIRECTION COSINES OF JOINT 
AXIS U. R. T. WORLD COOR. CR C3J J 


CRLL CRPO TO FIND RW - R X CVECTOR 
FROM END EFF. REF. PT. TO JOINTJ 


RJflCOB Cl. JT) 


RW Cl) 


RURCOB C2. JT] 


RW C2) 


RURCOB C3» JT) 


RW C3) 


RURCOB C4. UT) = fi Cl) 


RURCOB C5» UT) 


R C2) 


RURCOB CS. JT) = R C3) 


RURCOB Cl. JT) = 
ROT Cl. 1. JT. KflRM) 


RURCOB C2. UT) = 
ROT C2. 1. UT. KRRM) 


RURCOB (3. UT) = 
ROT C3. 1. UT. KRRM) 


RURCOB C 4. UT) 

= 0.0 


RURCOB C5. UT) 

= 0.0 


RURCOB CS. UT) 

= 0.0 


DO UNTIL. UT = NUMBER OF UOINTS IN RRM 

RETURN 

_ end ~~ 








1.3.20 DATOUT 


ORIGINAL PAGE eS 
OF POOR QUALITY 


DATOUT is responsible for the data output in the columns set up by sub- 
routine FORK of the E&S robotic simulation display. It includs the cur- 
rent simulation time, joint travel angles and percent of the maximum 
traveled for each joint. DATOUT has provisions for only two arms. 


OPEN n 

GRAPHICS SEGMENT 


INITIALIZE GRAPHICS 

LINE GENERATOR BLINK CAPABILITY 

4 


SUBROUTINE DATOUT 


SRVE CURRENT PICTURE PROCESSOR TRANSFORMATION ON STACK 


OUTPUT CURRENT 


DO FOR ERCH JOINT 


SIMULATION PROCESSING TIME 
IN ARM I 


OUTPUT JOINT TRAVEL RNGLE IN DEGREES 


COMPUTE PERCENT OF MAXIMUM TRAVEL TRAVERSED BY JOINT 


OUTPUT PERCENT OF JOINT TRAVEL LIMIT TRAVERSED* 

TRAVELLED *. GE. 95* 

I! 


PERCENT 


u« on non i 


CNULLJ 


R SECOND RRM EXISTS 


DO FOR ERCH JOINT IN RRM 2 


OUTPUT UOINT TRRVEL ANGLE IN DEGREES 


COMPUTE PERCENT OP MAXIMUM TRRVEL. 

TRRVERSED BY UOINT 


OUTPUT PERCENT OF JOINT TRRVEL 

LIMIT TRR VERSED 


percent travelled 

. GE. 95. 


’SET BLINK ON FOR QRRPHIC3 
TEXT OUTPUTTING PERCENT 


CNULL5 



CLOSE GRAPHICS SEGMENT 


R ETUR N 

END 
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1.3.21 FORM 


The FORM routine sets up the borders and the text output locations for 
the manipulator display on the E&S graphics unit. It sets up the Evans 
and Sutherland graphics display borders; outputs the robotic simulation 
title, current simulation time text title and joint travel status data 
column headings. FORM has provisions for only two arms. 


SUBROUTINE FORM 


SET VIRTUAL SPACE WINDOW 


□RRW GRAPHICS DISPLAY BORDER 


OUTPUT GRAPHICS DISPLAY TITLE 


OUTPUT MARTIN MRRIETTR COMPRNV LOGO 

* 

□RRW JOINT TRRVEL STRTUS FORM BORDERS 


OUTPUT UOINT TRRVEL STRTUS HERDINGS 


OUTPUT UOINT TRRVEL 

STRTUS COLUMN 


HERDINGS TEXT 

FOR RRM 1 


j\. SECOND RRM 

EXISTS 


OUTPUT UOINT TRAVEL STRTUS 
COLUMN HERDINGS TEXT FOR RRM 2 

CNULL3 


OUTPUT CURRENT SIMULATION TIME HERDING 


RETURN 


END 
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1.3.22 CYL 



Subroutine CYL is called within the system definition function during 
detailed graphic representation generation for the robotic system con- 
stituents (environment, arms, loads). If the requested component is a 
cylinder or cone, it is called to compute data points for the graphics 
routines. The controlling argument in the call, ISHAPE, determines 
which geometric shape has been chosen in calling routine BLDENV, BLDDAT 
or BLDENV. 


SUBROUTINE CYL 


PROMPT FOR CYLINDER DIRMETER. 

USED AS BOTTOM DIAMETER 

OF CONE 



ISHAPE - 2. FOR CONE 



PROMPT 

FOR TOP DIAMETER OF CONE 

| (NULL) 


PROMPT FOR LENGTH OF CYLINDER OR CONE 

SET NUMBER OF SIDES - © 

CALCULATE NUMBER OF LINES TO DRAW SEQUENTIALLY *“! 

SET 

LINE DRAWING MODE FLAG FOR CONNECTING SEQUENTIRL 

POINTS 

______ 

COMPUTE DELTR ANGLE FOR EACH SIDE OF THE CYLINDER 

SET ANGLE OFFSET PARAMETER 1 

ICOUNT - O | 

DO FOR EACH CYLINDER END CIRCLE 




CYLINDER RADIUS = DIRMETER/2. 


.. 

ISHAPE - 2- FOR CONE 



CYLINDER 

RADIUS - TOP DIAMETER/ 2* 

| (NULL) 


SET Nl_ = NUMBER LINES IN END CIRCLE ! 


DO FOR 

L— ■■ ... ..... 

ERCH VERTEX OF END 

CIRCLE OF CYLINDER 




ICOUNT - ICOUNT -p 

1 

COMPUTE ANGLE SUBTENDED USING ANG. 

L * _ 

OFFSET VALUE 


; COMPUTE V VRLUE OF POINT PS RRDIUS-COS (ANGLE SUBTENDED) 



COMPUTE H VALUE OF POINT AS RADIUS*SIN CANGLE 

SUBTENDED^ 


i 

SET X VALUE =* O. 


SECOND CYL. END CIRCLE 




!x - 

CYLINDER LENGTH VRLUE “J” 

(NULL) 



1 



LORD RRRRY. ELEMENT ICOUNT- WITH 

X. Y- Z 



u cwLOJurnt nunuw lzi«s to am pltwnbtely 


WET UXNK anon MODE Fi_«® FOR OOM ECTIN B RL.TERNWTXNO POINT® 


FROM a TO NUK 


INC W I CN T X 


ICOUNT - 1COUNT X 


LORO flRRRY. gUEMCNT ICCT-NT- UTTU X. Y. g PROM RRRRfV ELEMENT X 


LORO PPRWY* ELEMENT XCOUtMT. MXTH X. Y> B FROM nnnnY ELEMENT X^ O FF E gT 


Ld 


OF D^ flL PAG£ ® 
POOR QUALITY 
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1.3.23 RECT 


Subroutine RECT is called within the system definition function during 
generation of detailed graphic representations for environment, arm or 
load objects file. It is called if the requested component is a rect- 
angular solid (ISHAPE - 3), a symmetric trapezoidal solid (ISHAPE =4), 
or a nonsymmetric trapezoidal solid (ISHAPE “5) to compute data points 
for the graphic routines. 


SUBROUTINE RECT 



ORIGINAL PAGE & 

OF POOR QUALITY 
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1.3.24 TRISTR 


ORIGINAL PAGE [3 
OF POOR QUALITY 



Subroutine TRISTR is called within the system definition function during 
detailed graphics representation generation for the environment, manipu- 
lator or load objects. If the requested component is a triangular 
cross-section beam (ISHAPE * 6), it is called to compute data points for 
the graphics routine. 


SUBROUTINE TRISTR 







,SET LINE DRAWING MODE FLRQ FOR CONNECTING SEQUENTIAL POINTS 

I DO FOR EACH SEGMENT OF TRX. STRUCTURE. FROM 1 TO NUM. SIDES - 1 

CRLCULRTE OFFSET BASED ON CURRENT SEGMENT NUMBER 

j 

! 

X — CCURRENT SEG. NUM. — 13 (NUM. SIDES -h 1) 

LORD RRRRV X VALUE. ELEMENT 1 -►OFFSET. WITH X 

LORD RRRRY Y VALUE. ELEMENT ^OFFSET. WITH TRI. LENGTH/2. 

LORD RRRRY 2 VRLUE. ELEMENT 1 -►OFFSET. WITH O. 


! LORD RRRRY Y VRL-UE. ELEMENT 2+OFFSET. WITH —TRI. LENGTH/2. 


LORD RRRRY X VRLUE. ELEMENT 3*hOFFSET. WITH X 

LORD RRRRY Y VRLUE. ELEMENT 3+OFFSET. WITH O. 

LORD RRRRY Z VRLUE. ELEMENT 3^-OFFSET. WITH -TRI. LENGTH 

“Loro array x. y. * vfiuueau element 4m.offset. with elemt. i .♦offset values 

SET ICOUNT - NUMBER LINES RLRERDY DRRWN SEQUENTIALLY 


CRLCULRTE NUMBER OF LINES TO DRRW RLTERNRTELY 

SET LINE DRAWING MODE FLAG FOR CONNECTING RLTERNRTING POINTS 

SET ANGLE OFFSET PARAMETER TO CSEG. NUM. 3 «• (NUM. SIDES -•* 13 ™ 

[DO FOR EACH TRIANGLE SIDE FROM 2 TO NUM. SIDES 




ICOUNT - ICOUNT 1 

LORD RRRRY. ELEMENT ICOUNT. WITH X. Y. Z FROM RRRRY ELEMENT I 

ICOUNT - ICOUNT -h 1 

LORD RRRRY. ELEMENT ICOUNT. WITH X. Y. E FROM RRRRY ELEMENT I-K3FF3ET 


Hi 
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1.3*25 DATATAB 


Subroutine DATATAB is called within the system definition function 
during detailed graphics representation generation for the environment, 
the robotic arm or load. If the requested component is a data tablet 
structure, it is called to compute data points for the graphic routines 
when the input ISHAPE flag * 7. 


SUBROUTINE DRTRTRB 

PROMPT ro w NUMBER OF POINT g TO PC INWJT VXW P*T« Tt 

INlTIPL-IMg C3RTP TRLIT PgVXCg 

TNinwuag gvgNT ak ue 

” KOUNT -6 _ 


DO UNTIL. PUL. POINTS HAVE MIN INPUT 

PLOT LP3T POINT 

BgflD 2 POINTS FROM DPTT^~ TAB. BCTUC CN UHICH THERE IS « KNOUN CCTTRNC g 

PROMPT USER fOR KNOWN OlHCN glON BETWN, 2 PTS, . USCO POP 3CP1-XNG 

COMPUTE - INTERNAL OPTP TBB. OISTRNCg agTWggN 2 POINT? 

9CALE FPC' lO R - DlMCNaiON/OPTR TRflL INTERNPL DiaTBNCE 

” REPO POINT FROM DR TP TRB. TO BE USED R3 ORIOXN 


PROMPT FOR SHRPE DEPTH VRt-UC 


CPLCULflTE NUMBER OF LINES TO DRRW 3EQUENTXRLLY 

Line drrwinq mode flpq for con^ctxnq sequentxfl points 


LORO RRRRV g» gL-gMgNT ICOWT. 

CALCULATE NUM BER OF LINES TO DRAU STERNA TELV 

SET LINE DRAWING MODE FLRG for conne cting alternating poin ts 

SET ANGLE Of' E3 ET PARAMETER TO NUM. 3IDE3 •+» 1 

DO FOR EACH FIGURE SIDE FROM 2 TO NUM. SIDES 

~ xncreme:nt x 

~ ~~ ICOUNT - ICOUNT 1 

LORD RRRRV. ELEMENT ICOUNT, WITH X , V. Z FROM ARRAY gkg ME , ^T . __S 

— ICOUN T * - ICOUNT ^ 1 

LORD RRRRV, ELEMENT X C OUNT . WITH X, V, g FROM RRRRV ELEMENT 

return 

— — “* “ cnET 


UR!G:NAL PA2S IS' 

of poor quality 



1.3.26 FILLET 


ORffm*U r- - ^ i-- 

^ K I v - Am u A ■ •.; _ ■ L ; r% 

of poor Quality 



Subroutine FILLET is called within the system definition function 
during detailed graphics representation generation for the environment, 
the robotic arm or load. If the requested component is a fillet part, 
it is called to compute the data points for the graphic routines. For 
a concave or convex fillet, the input ISHAPE flag is 8. 



SUBROUTINE FILLET 










■■ ■ 

j CnLCULBTE NUMBER OF LINES TO DRRW SEQUENTIALLY 



SET LINE DRAWING MODE FLAG FOR CONNECTING SEQUENTIAL POINTS" 







[DO FOR EACH FILLC T END DIAMETER 

i SET FILLET RRPIUS - RRDXUS INPUT 


SET NL 


NUMBER LINES IN END DIAMETER 


DO FOR EffCH VERTEX END PERIMETER. FROM 1 TO NUM. SIDES 


| 


ICOUNT - 

ICOUNT -i- 1 


jCOMPUTE 

PNGLE 

SUBTENDED USING DEI_Tfl RNG. VRLUE 


(COMPUTE V 

VALUE OF 

POINT RS RRCL 

- RRO* #COS (ANGLE SUBTENDED 


[compute h 

I 

VALUE OF 

POINT RS RAO. 

- RRO. ~SIN CRNQLE SUBTENOEDJ 


SET X VALUE « O. | 



SECOND FILLET 

END PERIMETER 

^ri 



FILLET 

LENGTH 

T~ (NULL) 


LORD 

RRRRY. 

ELEMENT 

ICOUNT. WITH X. V. 2 

J 


]_. ZNCAKfroosrr x ' ] 

xcoltit - icount ♦ i - ... | 

tow rwwr# ELO«WT ICOUNT* WITH X* Y* W rWH m 
ICOUNT ■ IOQLNT ♦ 1 

wrwr ELEMENT X 


LORO wwwv. klkmbnt xcoumt. wxtm x. y. b fkm wpwwv 

wnm 

ELEMENT Z4CPFWT 

-id 
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1.3.27 OBSTCL 


Subroutine OBSTCL is called within the system definition function 
during graphics representation generation with option ISHAPE equal 9 
for the environment, detailed robotic arm or load. It is called ir a 
requested component is an obstacle entity (a choice option from BL ENV) 
or nonplanar structure (for BLDLOD or BLDDAT). It computes data p,ints 
for the graphic routines. 


SUBROUTINE OBSTCL 


fc»OMPT USER FOR CHOICE TO 


COMPONENT FH_E OR INPUT IN i .jh-L. f IVgi-Y 


RERDING FILE 


PROMPT USER 
FOR COMPONENT 
FILENAME 


OPEN 

COMPONENT 

FILE 


prompt fob number op points to be 

O B S T C L. PERIMETER 


INPUT POR 


SET MUM. SIDES — NUM. RTS. TO BE INPUT 


KOUNT = O 


RERD NUMBER OF 
DRTR POINTS 
VRI—UE FROM FILE 


KOUNT = KOUNT 


DO UNTIL RLL POINTS HRVE BEEN INPUT 


KOUNT 


NUMBER RTS. RERD 


3 


■ 

RERD RECORD TROM 
FILE CONTRININC 

SET LRST DRTR RRRRY ELEMENT. WITH 
X. Y. Z VALUES RERD FOR ELEMENT 1 

■ 

X. Y. Z VRLUE3 OF 
PT. XNTO DRTR 

PROMPT USER FOR SCRLE FACTOR TO 

BE USED 

DO 

UNTIL RLL 

-t-k fre i io\ /cr 

SET LINE DRRWINC MODE EUR© EOR CONNECTING 
SEQUENTIAL POINTS - KOUNT 

j t— 'LJ-L.1N i co nn i v i— 

IBEEN RERD 

SET LINE DRRWING MODE FLRG FOR 
CONNECTING RLTERNRTING POINTS «■ O 

1 

CLOSE 

DO FOR ERCH POINT TO BE CONNECTED 
SEQUENTIALLY 

LUMKUINC.IN 1 

FILE 

LORD RRRRY X* Y. Z. ELEMENT KOUNT. WITH 

DRTR X. Y. Z«* SCRL. FACT | 


RETURN 

END ! 


■j i-siLii! « AL PAll 5*3 

OF POOR quality: 
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1.3.28 ORIENT 


ORIENT is called from most of the build options in INITDRVR, allowing 
the user to reposition components. The user can input a rotation se- 
quence consisting of rotation axes and angles, and a translation vector 
to position the origin of the component within the reference coordinate 
system. MAT is called to compute the total rotation transformation ma- 
trix, and MATVEC to transform vectors from the new coordinate system to 
the reference system. The translation vector is then added to each set 
of coordinates. 


SUBROUTINE ORIENT 


RSK USER IF ROTRTIONS RND TRANSLATION 
RRE REQUIRED OR ONLY TRANSLATION 


* 

V 

ROTRTIONS RRE REQUIRED 

/ 

4 

I 

PROMPT USER FOR ROTATION 

AXIS 



1 

PROMPT USER FOR ROTRTION 

ANGLE 



DO 

UNTIL. USER STOP 



CALL MAT TO CALCULATE 
TRANSFORMATION MRTRIX 



CALL MRTVEC TO TRANSFORM LINES OF 
GRRPHICS REPRESENTRTION 




PROMPT USER FOR X. Y. RND Z TRANSLAIONS 


ADO TRANSLATIONS TO GRAPHICS REPRESENTATION 

VARIABLES 


RETURN 


END 
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1.3.29 MAT 


Subroutine MAT is called during the system definition function to com- 
pute the total rotation transformation matrix defined by the input ro- 
tation sequence and angles. MAT is called from subroutine ORIENT. The 
rotation sequence passed to it determines the transformation matrix 
from the component system to the reference system it calculates. The 
transpose (inverse) of the normal X,Y and Z-axis rotation matrices are 
used. For each rotation in the input sequence, the axis rotation matrix 
is loaded and premultiplied with the current total transformation 
matrix. 


SUBROUTINE MRT 

x - Q 

i - x -*• i 

H NS - ■— INPUT ROTATION HNS TO OWXENT LINK IN CWWENT COOWP. «Va. 

ZERO RMRT INTERMEDIRTE ROTRTION MRTRIX 
"vT ROTRTXON RXXS XS X— RXXS 




1.3.30 MATVEC 



Subroutine MATVEC is called during the system definition function to 
provide matrix/vector multiplication. The routine is called from 
ORIENT. The vector A is multiplied by the matrix TRANS to produce out- 
put vector B. Note that this matrix/vector multiplication is 3-D only. 


SUBROUTINE MATVEC 


1 = 0 


I i 


U = O 


B Cl) = PRODUCT OF MATRIX TRANS. 
ELMT. Cl. UD VECTOR R. ELMT. CUD 


DO UNTIL. U . EQ. 3 


jDO UNTIL. I . EQ. 3 


RETURN 


END 
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1.3.31 DRAW 


Subroutine DRAW is called within the system definition function to pro- 
vide the graphics display during the generation of arm, load or environ- 
ment detailed representations. It is called to display each successive 
component as it is defined. The routine logic is controlled by flag in- 
puts specifying initialization (at which time base/link/tool transforma- 
tion matrix concatenations to the system are performed), component draw- 
ing, or component modification world. 



SUBROUTINE DRRN 


SET PROCESSOR MODE — 1. FOR SYSTEM DEFINI T ION 

SET SCALE FO CTOR. IFRCT « lOOQ. /RRM SPRN 

INITIRLI2ING DISPLAY A 


" FIRST L-XNK COR BPS® 

oo roR ewae, all links and tooi_ 

CNULL3 

SET PXCTURE ! 

PROCESSOR TRANS* 

TO IDENTITY 

cm i m m wrr to mrr up- i m r^jxma for l 

ggno TNTEtiefi TRflNSLFTION AND ROTATION VAt-LCS 

CALL DIALS TO 
STATUS ANALOG 
CONTROL- DIALS 

■ ■ ■ — — — s 

DX3PLAY WINDOW SPAN — SCALEQ RRM 3PRN 

9ET WINDOW BOUNOARXES 

rm [ <•» rrr*(DlNflTPD AA9C AXES SYSTEM CHCD-X. WHET-Y. BLLWD 

^ DRRNING NElUI OR REPLACING COMPONENT 


I — I *■« l— > I < 1 ■ — ' - 

DISPLRY NXNDOUJ SPRN = SCRLED RRM SPRN 


1 SET WINDOW BOUNDARIES ___ 

[SET CURRENT PICTURE PROCESSOR TRRNSFORMflTION 


DO FOR RLL. LINKS CETC. 1 AND COMPONENTS BEFORE CURRENT COMP, 


DRRW COLOR COORDINATED RXES SYSTEM CRED— X. WHIT- Y. BLU— 2D 


SET NUMBER OF COMPONENTS XN LINK PARAMETER 

DO FOR EACH COMPONENT IN LINK 


SET GRAPHICS FLAGS FOR SEQUENTIAL RND ALTERNATING PTS. 

CRLL D3DRTR TO DISPLAY COMPONENT 

SET COUNTER FOR LAST RRM OBJECT ARRAY LOCATION USED 

SET GRAPHICS FLAGS FOR SEQUENTIAL AND ALTERNATING PTS* 

030RTR TO DISPLRY COMPONENT 

SET COUNTER FOR LAST RRM OBJECT ARRAY LOCATION USED 


CLOSE RND REPLACE SEGMENT 


RETURN 

END 


ovmK ■ ^ 

OF FOOL QL^UTsf 
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1.3.32 ESMAT 


Subroutine ESMAT uses Evans and Sutherland graphics routines to con- 
struct the required transformation matrices from each system section 
coordinate system to the graphics coordinate system. Input argument K 
specifies which system section is under consideration. It is called 
during execution of the system definition function. It is called from 
subroutine DRAW to compute the required transformation matrices for each 
system section. The robotic system has section coordinate systems for 
the base, each joint/link, and the end-effector. An input value of K*1 
indicates the robotic system base. The transformation matrix is com- 
posed of a translation matrix based on the base location and rotation 
matrices constructed using the base orientation parameters. A value of 
K from 2 to the number of links plus 1 (NJ+1) indicates the (K-l)th 
joint/link. All transformation matrices from each of the sections to 
the previous joint (or base, if the current joint is the first joint) 
are concatenated to form the total transformation matrix to the graphics 
coordinate system. Each joint transformation matrix is composed of a 
translation matrix based on the joint position, a rotation matrix based 
on the initial joint angular displacement, and rotation matrices for 
joint orientation. A value of K=NJ+2 indicates the end-effector system. 
The transformation matrix for the end-effector is composed of a trans- 
lation matrix, and rotation matrices for end-effector orientation. The 
end-effector location and orientation are specified relative to the 
coordinate system of the final joint in the system. 


SUBROUTINE ESMRT 


RRM BASE 


■BBS 

3Cnt _E BfW LOCATION BY IFRCT AND LORD XNTO INTEGER RRRRY 


MZHI 



' — j 



, 

[EXTRACT X. Y. 55 ROT- ANGLE'S USED IN ORIENTING- FROM UT. ANGLE ARRAY 



- 


2E 




ONE OF THE RRM JOINTS 




CALL TTRAN FDR PICTURE PROCESSOR TRANSFORMATION MATRIX 


EXTRRCT OFFSET JT. RNG. FROM JOINT VRRXflBLE RRRRY "~ 1 

- — . 

CALL TROTX OR — Y WITH JT. PNG. TO ROTRTE TRANSFORMATION 

— ! 

EXTRACT ROTATION AXES USED IN ORIENTING- FROM UOINT SEQUENCE RRRRY 

zr? 

EXTRRCT X- Y. Z ROT- ANGLES USED IN ORIENTING. FROM JT. ANGLE RRRRY 

cfE; 

CALL TROTX- -Y OR -Z WITH INTEGER RNG- TO ROTRTE TRANSFORMATION 


CALL TGET TO LORD MATRIX ARRAY WITH CURRENT PICTURE PROCESSOR TRANS- 


>\ RRM TOOL 

mmm 

SCALE TOOL LOCATION BY IFRCT RND LORD INTO INTEGER 



RRRRY 



CALL TTRAN FOR PICTURE PROCESSOR TRANSFORMATION MATRIX 



EXTRRCT ROTRTXON RXES USED IN ORIENTING* FROM JOINT 


‘ * 

SEQUENCE RRRRY 


j 

EXTRRCT X. Y. Z ROT- RNGLES USED IN ORIENTING* FROM JT. 


— i 

ANGLE RRRRY 



CALL TROTX* -Y OR -Z WITH INTEGER RNG. TO ROTRTE 



TRRNSFORMRTION 



CALL TGET TO LORD MRTRIX RRRRY WITH CURRENT PICTURE 



PROCESSOR TRANS. 



RETURN 


END 
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1.3.33 DBAS 


Subroutine DBAS is called within the system definition function during 
detailed graphics representation generation. For the subroutine, input 
calling argument IMAN specifies environment, robotic system component 
or load objects file consideration. Graphics object data IOBJBK are 
loaded for robotic system components, LGRAFBK is loaded for load compo- 
nents, and ENVTBK is loaded for environment components. The manner in 
which the data are stored in the COMMON blocks is dictated by the data 
format used in Evans and Sutherland graphics routine D3DATA. 


SUBROUTINE DBRS 

FIRST COMPONENT XN DISPLAY SYSTEM 


DOCRSE 

DEFINING ROBOT PRM 

SET SCRLE FACTOR - lOOO. /ARM 

SPAN 

SET NUMBER OF COMPONENTS IN 
CURRENT LINK PARAMETER 


DBFZNINQ fNVIBONMENT 

SET NUMBER OF 
■ COMPONENTS IN 
ENV. PARAMETER 




LOPO PPM OBJ. PARRY. CLEMENT 2* THRU 3 
ICOUNT. US LINE COf^CCT FT-R03 


ICOUNT = ICOUNT 3 


DO FOR EACH SEQUENTIAL PT. 


L-OHO PRN OBJ. PRft ■ BLMNTS. i-* TUB J 21 

icount. us me pL-mn bbm o*u. ort« 


ICOUNT = ICOUNT -h 3 


bltctnrtin» points 


:ount 


ICOUNT 


DO FOR EACH ALTERNATING PT. 

LORO PRM OOJL PRR. • O-MNTS. TMRU 3-*- 

ICOUNT. SCALED ARM OftJ. DPTO 

ICOUNT = ICOUNT - 3 

SET COUNTER FOR LAST ARM 
OBJECT ARRAY LOCATION USED 


LOAD ENV. OBJ. 

ARRAY FOR 
GRAPHICS FLAGS 


LOAD ENV. OBJ. 
ARRAY FOR 
SEQUENTIAL 

POINTS 


LOAD ENV. uBJ. 

ARRAY FOR 
GRAPHICS FLAGS 

' LORO ENV. OBJ. 
ARRAY FOR 
ALTERNATING 
POINTS 

SET COUNTER 
FOR LAST ENV. 
OBJECT ARRAY 

location used 


SET SCALE FACTOR 
- lOOO. /LOAD SPAN 


SET NUMBER OF 
COMPONENTS IN 
LOAD PARAMETER 


SET START 
LOCATION IN LOAD 
OBJECT ARRAY FOR 
CURRENT COMPONENT 

LOAD LOAD OBJ. 

ARRAY FOR 
GRRPHICS FLAGS 

LOAD LOAD OBJ. 
ARRAY FOR 
" SEQUENTIAL POINTS 


LOAD LORD OBJ. 
ARRAY FOR 
“ GRRPHICS FLAGS 


LOAD LOAD OBJ. 
ARRAY FOR 
ALTERNATING 

- POINTS 

SET COUNTER FORT 
LAST LOAD OBJECT 
ARRAY LOCATION 
USED 


■ 

■ 

■ 


RETURN 


END 
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1.4.1 CVTUNIT 


Subroutine CVTUNIT is responsible for the conversion of input data from 
I/O units to internal mathematical units. Each data value VAL is multi- 
plied by CONUNIT(IDIM) and replaced in VAL. 


SUBROUTINE CVTUNIT 


□O FOR ERCH VALUE TO BE CONVERTED 


VALUE 




— VALUE TIMES RPPROPRIRTE 
COMPONENT OF CONUNIT 
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1.4.2 MATMPY 


Subroutine MATMPY performs the multiplication of two matrices, AB^C, 
where A has I rows and J columns, the dimension of B is JxK and C is 
IxK. The matrices and their sizes are passed to subroutine as calling 
arguments. 



SUBROUTINE MRTMPY 
C CIROW. ICOU) — O. O 


R = R CIROW- ICNT) -*-B (ICNT. ICOU 


C CIROW- ICOU =» C CIROW- ICOL] -*-R 


DO UNTIL ICNT = NUMBER OF 
COLUMNS IN MRTRIX R CROWS IN B) 


□O UNTIL ICOL = NUMBER OF 
COLUMNS IN SECOND MRTRIX B 


DO UNTIL IROW = NUMBER OF ROWS IN 
FIRST MRTRIX R 


RETURN 


END 



1.4.3 ERRMSG 


Subroutine ERRMSG is called when certain errors occur during ROBSIM 
execution. The routine first displays the current operating mode 
(i.e., system definition, analysis, or postprocessing). The routine 
searches the file ERROR.DAT for an error message corresponding to the 
error number passed to it. The message is typed at the terminal and 
execution returns to the calling routine, from which it continues or 
terminates depending on whether the error is fatal. 
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1.4.4 ROTMAT 


ROTMAT computes a rotation matrix from a sequence of up to three rota- 
tions about coordinate axes. It decomposes the input calling argument 
JSEQ into three successive rotation axes, computes each corresponding 
rotation matrix from the specified angles of rotation and combines these 
successively to find the overall rotation matrix. 


SUBROUTINE RQTMRT 

INITIALIZE RRRPY T2 C3. 3) TO RN 

IDENTITY M RTRIX 

DETERMINE INDIVIDUAL ROTATION AXES 

CIJO C3) ) 

X IJO Cl) . NE. O 


CPL_L CETM TO FIND THE 
TRANSFORMATION MRTRIX 
T 1 (3. 33 FOR R SINGLE ROTATION 

CRLL MRTMPY TO COMBINE 
NEW MRTRIX T1 WITH OLD 
MRTRIX T2 


CNUL_1_) 


DO UNTIL. 1=3 


PUT RESULT IN ARRAY RM C3. 3) 


RETURN 



1.4.5 


CETM 


Subroutine CETM calculates a transformation matrix for a specific input 
axis of rotation and rotation angle by the use of appropriate direction 
cosines matrix. The calling argument input is: 

Symbol Type Dim. Definition 


IAXIS 


TH 


1*4 1 Rotation axis for joint orientation 

* 1, Rotation about x-axis 

* 2, Rotation about y-axis 

* 3, Rotation about z-axis 

R*4 1 Rotation angle for joint orientation 


SUBROUTINE CETM 
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1.4.6 LOGO 


The LOGO routine calculates data points required to output the Martin 
Marietta logo, and displays it on the robotic simulation E&S graphics 
display. It extracts from the data points file LOGO.DAT, the Martin 
Marietta company logo, scales and displays the logo for the robotic 
graphics simulation. 




SUBROUTINE LOGO 

~ OPEN LOGO DRTR FIL-E 

REflO RL_L_ OF LOGO DRTR INTO INTEG ER DRTR RRRRY 

CLOSE LOGO DR TR FIL-E 

SET INTEGER STEP VHLUE TO 8Q/INPUT SIZE 
|dO FOR'ERCH DRTR HORIZONTRL P IXEL- 

zzz zzi : 

| INCRE M ENT X 

! INTEGER X. V VALUES — CD AT A flffRflY C—MT. «*3>JFUT SXSE) ♦ INXTXBt. X. Y 


! INTEGER Z VRLUE = INITIAL Z 


L_ = L_ -+- 1 


SET OUTPUT LOCO RRRflV X. V. Z. ELEMENT L. TO X. V. Z 
SET START RND STOP POINTS LOOP PRRRMETE RS 

: U as Q J 

DO WHILE L. NUMBER OF POINTS IN LOGO. . LE. 500 

U ■= «-J 1 

DETERMINE RRTXo"toT|3E IN OUTPUTTING LOGO POINTS 
REWRITE INTEGER X VRLUE — INTEGER X •» RATIO 

pf? UNTIL- J . OT- STOP LOOP PPPM- • PHAQM #TPRT L O OP PPPH. • AT ®TEP VPLLE 
;3E t TlN E GENERRToFFL^rFO R GR ^HIcF ^ISPLflY^ 
iDlSPLRY CURRENT GRAPHICS DRTR RRRPY " DRRI4 SCALED LOGO 


RETURN 


END 


B-9 


1.4.7 CRPD 




Subroutine CRPD computes the cross-product of two vectors A and B, each 
containing three components. The result is put into the vector C. 


SUBROUTINE CRPD 
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The Analysis Tools Function 


The program SIMDRVR is the analysis tools function driver. The following 
set of routine functional descriptions and VCLRs (visual control logic repre- 
sentations) are the modules found in the analysis tools function of ROBSIM. 



Figure B-8. - Functional block diagram for SIMDRVR. 
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blank not filmed 
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TABLE B-VI . - PROGRAMS EMPLOYED IN SIMDRVR 


2.0 

SIMDRVR 

2.2.22 

INTGRT 

2.3.26 

FORTOR 



2.2.23 

ENDSIM 

2.3.27 

FORREF 

2.1.1 

RDSYS 



2.3.28 

CMP CTRL 

2.1.2 

SGMNT 

2.3.1 

SEGINIT 

2.3.29 

PIDCON 

2.1.3 

REQUIR 

2.3.2 

GRASP 

2.3.30 

PIDFOR 

2.1.4 

RESPON 

2.3.3 

RELEAS 

2.3.31 

EFINRT2 



2.3.4 

ESCNTRL 

2.3.32 

NLINK 

2.2.1 

FTIN 

2.3.5 

POSSPJT 

2.3.33 

SIMLMT 

2.2.2 

REQOPT 

2.3.6 

RCNTRL 

2.3.34 

STOPFR 

2.2.3 

PRTARM 

2.3.7 

RATEPRO 

2.3.35 

ACTIVPIH 

2.2.4 

GRAFIX 

2.3.8 

PCNTRL 

2.3.36 

DRTORQ 

2.2.5 

CNTRLR 

2.3.9 

CABSM 

2.3.37 

EFINRT 

2.2.6 

SPRGINC 

2.3.10 

FORCE 

2.3.38 

SLVTHDD 

2.2.7 

CHKLMT 

2.3.11 

TORQUE 

2.3.39 

LININT 

2.2.8 

DYNAM 

2.3.12 

ACTORQ 

2.3.40 

LDVOLT 

2.2.9 

VOLTAGE 

2.3.13 

REQPRT 

2.3.41 

CALCI 

2.2.10 

OUTREQ 

2.3.14 

REQSOF 

2.3.42 

SOLVE 

2.2.11 

ESPAUS 

2.3.15 

REQTRQ 

2.3.43 

GAUSS 

2.2.12 

ENDREQ 

2.3.16 

REQPLT 



2.2.13 

SIMOPT 

2.3.17 

POSGRDJT 

2.4.1 

SLVLIN2 

2.2.14 

INITCO 

2.3.18 

JTPOS 

2.4.2 

REPCOL 

2.2.15 

DEFCNST 

2.3.19 

CVTIN 

2.4.3 

ORERR 

2.2.16 

PIDINIT 

2.3.20 

SPRGFOR 

2.4.4 

OUTUN 

2.2.17 

OUTSIM 

2.2.21 

CNSTFOR 

2.4.5 

ICVTATD 

2.2.18 

CNTRSIG 

2.3.22 

PTACC 



2.2.19 

CONTROL 

2.3.23 

POSSENS 



2.2.20 

SETCNST 

2.3.24 

SIMPRT 



2.2.21 

DERIV 

2.3.25 

SIMPLT 
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2.0 


SIMDRVR 


The program SIMDRVR is the analysis tools function driver. It operates 
in an interactive mode, prompting the user for the analysis option de- 
sired: requirements analysis without graphics, requirements analysis 

with graphics (a display of system motion during program execution), re- 
sponse simulation analysis without graphics, response simulation analy- 
sis with graphics, option to set up a motion program or terminate 
SIMDRVR execution. 


PROGRAM SIMDRVR 


SET PROCESSOR MODE - 

2 

. FOR ANALYSIS TOOLS FUNCTION 

SET 1 

ERROR CODE = O 

CRLL SETLU TO SET PROGRRM DEFAULT LOGICAL UNIT NUMBERS 


PROMPT USER FOR OPERRTION MODE, XMODE 


READ USER RESPONSE 


IMODE - 


Cflt-L. ROSY3 TO OPEN. REBD. RNO CL O S E SYSTEM OflTB FILE 
GI — 

co ££ do CRSE ON OPERRTION MODE 


Ili 


rr 

£5 c_> 


□OCRSE 


IMODE 
. LE. 2 


XMODE . GT. 

2 . RNO. 
XMODE . LT. 5 


IMODE 


* V3XS WITH OR 

out onnm rrr« 


KIHUUmDN WITH 
OR WITHOUT 


COLL SGMNT 
TO SET UP R 
MOTION 
PROFILE 
PROGRAM 


WRITE 

ERROR 

MESSRGE 


DO UNTIL OPERRTION MODE DESIRED IS TO TERMINATE SIMDRVR EXECUTION 


STOP 


END 


ORIGINAL PAGE IS 
OF POOR QUALM . 
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2 . 1.1 


RDSYS 


Subroutine RDSYS is called from SIMDRVR to input the manipulator system 
definition data needed to run any of the SIMDRVR analysis options. The 
routine first prompts the user for the name of the file containing the 
system's data and then opens that file. Geometric properties, mass pro- 
perties, actuator properties, and special joint data for each arm are 
read in, as well as system graphics data and the definition of gravity 
for the system. If the system contains an environment, the data descri- 
bing it are read in. If load objects are also to be included, the data 
describing them are read in. End-effector data for each arm is the last 
information to be read. After that, the file is closed and saved. 


SUBROUTINE RDSVS 


| PROMPT FOR NRME 

OF SYSTEM 

DRTR FILE 


OPEN SYSTEM 

DRTR 

FILE 


| RERD INPUT 

RND 

OUTPUT UNITS 


RERD NRRM (NUMBER OF 

RRMS 

IN SYSTEM] 



READ ARM GEOMETRIC PROPERTIES 


RERD RRM MWgS PROPERTIES 
RERO RCTURTOR PROPERTIES 
RERD SPECIRL. JOINT DRTR 


DO UNTIL KRRM = NRRM 



RERD SYSTEM GRAPHICS 

DRTR 


RERD GRRVITY 

NUMBER OF ENVIRONMENT COMPONENTS 

. NE. O 


RERD 

ENVIRONMENT DRTR 

(NULL] 


NUMBER OF LORD OBUECTS 

. NE. O 


RERD LORD OBUECTS DRTR 

CNULL3 

RERO 

LORD OBUECTS GRAPHICS DRTR 



RERD TOOL DRTR FOR ERCH 

RRM 


DISPLAY MESSAGE THAT FIL-E RERD AND COMMON 

BLOCKS LOROEO 


CLOSE RND SRVE FILE 


RETURN 

* 



END 
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2.1.2 SGMNT 


Subroutine SGMNT allows the user to set up the desired motion profile 
for a requirements analysis or response simulation run. It is called 
from SIMDRVR. An existing motion profile file may be read in and modi 
fied or the profile may be defined interactively. Motion is specified 
in one of four ways: 


1) Desired position of end-effector; 

2) Desired position of each joint; 

3) Rate of end-effector movement; 

A) Rate of each joint. 


Several nonmotion— type operations such as grasp a load object, release 
object and wait a given length of time may also be specified. 


SUBROUTINE SGMNT 


DOCRSE 


MODIFY EXISTING FILE 


INPUT DRTR FROM 
TERMINAL 


PROMPT USER FOR 
FILENAME 


PRINT 

INITIAL 

UOINT 

ANGLES 

ALLOW 

USER TO 

MODIFY 

’ ANGLES 

CALL 

SETUP 


PROMPT USER 
FOR FILENAME 


RERO RND DISPl—RY 
CURRENT SEGMENT DRTR 


PROMPT FOR 

INITIAL 

UOINT 

ANGLES 

CALL 

K 

SETUP 
7 


RLLOW USER TO MOD. 
DELETE OR INSERT DRTR 



DO UNTIL ALL EXISTING 
SEGMENTS REVIEWED 


ENTER MORE 
SEGMENTS 


CNULL3 


PROMPT 
USER FOR 
SEGMENT 
DRTR 



PRINT R 
FILE 


CL CO I — I 


CO 


CD 


DO UNTIL USER 
STOP 


DO UNTIL 
USER STOP 


£ £ 

CO 


CO 


CD 

cn 

co 

co 


CD 

CO- 
CO 
I — I 
CD 


DO UNTIL KflRM . EQ. NUMBR OF RRMS 

RETURN 
END 
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2.1.3 REQUIR 


Subroutine REQUIR is called from SIMDRVR and Is the routine that con- 
trols the execution of any requirements analysis run. It first calls 
REQOPT to set up program run time options. If requested PRTARM is 
called to write a description of the system to an output file. SETUP 
is called to calculate initial positions. GRAFIX is called if the run 
is to include graphic displays. The subroutines CNTRLR, SPRGINC, 

CHKLMT, DYNAM and OUTREQ are called at every increment of a user-defined 
time loop to calculate the manipulator system's motion, forces and tor- 
ques, and write these data to- an output file. ESPAUS is called when 
motion is temporarily halted during execution. When the stop time is 
reached, ENDREQ is called to close any open files. 


SUBROUTINE REQUIR 


! CR!_I_ REQOPT 

TO 

SET OR MOD PROGRRM OPTIONS 





IPRINT 

- LE. 2 


CRLL. 

PRTRRM TO WRITE ORTR 

TO FTL.E 

(NULL) 





IMODE 

. EQ- 2 



NULL 

SET GRAPHICS FLOGS | 





CRLL GRRFIX | 



CRLL CNTRLR TO COMPUTE MOTION FROM TIME HISTORIES 


CALL SPRGINC TO GET SPRING FORCES IF NEEDED 

IDYNM . EQ. 1 

/f 

CRLL CHKLMT 

CRLL CHKLMT 

CRLL DYNRM FOR DYNAMICS 

CRLL OUTREQ TO OUTPUT RESULTS 

IMODE . EQ. 2 


NULL 

SET GRAPHICS FLRGS 

RLLOW USER TO STOP MOTION 
RND VIEW SYSTEM 


DO UNTIL TIME . EQ. STOP TIME 


CRLL ENOREO TO CLOSE FILES 


NULL 


IMODE . NE. 


RETURN 


COLL GRRFIX 


END 
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2.1.4 RESPON 


Subroutine RESPON is called from SIMDRVR to control the execution of a 
response simulation run. Run time options and program variables are 
first initialized. A user-defined time loop is executed to call rou- 
tines to carry out all the control functions. After execution is com- 
pleted, ENDSIM is called to close the files. 



SUBROUTINE RESPON 


cnuu 

SIMOPT TO DEFINE PROGRAM 

RUN OPTIONS 


cri_l. 

INITCO TO DEFINE INITIAL 

CONDITIONS 


I'cRI-L. DEFCNST TO DEFINE R CONSTRAINT PLANE IF DESIRED 1 

PID CONTROL IS USED 



FORM 

INITIAL STATE VECTOR AND 

DERIVATIVE 



ARM OUTPUT DATA REQUESTED 

^ 

jcRL-U RRTWWM TO WRXT* XNITXRL. RRM C3RTR | 

CNULJ-D 


f INITIALIZE TIME. TOL. AND THDD J 

1CRL.L- DYNRM FOR INITIAL DYNflMICS 

CRLCULATIONS 



r CALL GRRFIX IF GRAPHICS REQUESTED ! 

CALL OUTSIM TO WRITE START 

TIME DATA 

— 


PID CONTROL. IS USED 


CRt-L cnt naxm * control^ rnd sctcnst | 

(NULL) 


reset STATE VECTORS AND DERIVATIVES IF NECESSARY 

CALL INTGRT TO PERFORM 

INTEGRATION 


CALL ESPAUS AND GAAFIX IF GRAPHICS ARE IN USE 

CALL DYNRM FOR DYNAMICS 

CALCULATIONS 


SET END EFFECTOR FORCES AND TORQUES 

CALL OUTSIM TO WRITE 

OUTPUT DATA 




□O UNTIL TIME . EQ. STOP TIME 


COLL ENDSIM TO CLOSE FILES 


CALL SRRFIX TO TERMXNRTE GRAPHICS XF USED 


RETURN 


ENO 


C’’:-' '‘{N/’.l, PAGk 

Of 'rOOii QUALITY 
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2.2.1 FTIN 


Subroutine FTIN is called from SGMNT if force/torque or active compli- 
ance control was specified by the user. If force/ torque control was 
specified, the user is prompted for the number of force and torque com- 
ponents to be controlled, the unit vectors in the directions to be con- 
trolled and the magnitude of the control force or torque. If active 
compliance control was specified, the user is prompted for the stiffness 
matrix at the end-effector reference point. 


SUBROUTINE FTIN 


X IOP . EQ. S 


PROMPT USER FOR FORCE/TORQUE CONTROL. ON/OFF 

FLAG 


X TURN FORCE/TORQUE CONTROL ON 


PROMPT USER FOR NUMBER OF FORCE 
COMPONENTS TO CONTROL. 


PROMPT USER FOR CONTROL DIRECTION 


PROMPT USER FOR MAGNITUDE OF CONTROL FORCE 


DO UNTIL N . EQ. NUMBER OF CONTROLLED 
FORCE COMPONENTS 


PROMPT USER FOR NUMBER OF TORQUE 
COMPONENTS TO CONTROL 


PROMPT 

USER FOR CONTROL. 

DIRECTION 

PROMPT 

USER FOR MAGNITUDE 
TORQUE 

OF CONTROL 


DO UNTIL N . EQ. NUMBER OF CONTROLLED 
TORQUE COMPONENTS 


N. . IOP - EQ. S 


PROMPT USER FOR COMPLIANCE CONTROL. ON/OFF FLAG 


N TURN COMPLIANCE CONTROL ON 


(NULL) 


RETURN 


END 


P 

IS 


i r IkL^u L i 



CNUL1—3 



















2.2.2 REQOPT 


Subroutine REQOPT is called from REQU1R to define requirements analysis 
run time options. The user may list current options and use them or in- 
put a new set of options. Options the user may set include run time 
data file write, file of joint torques, control method to be used, exe- 
cution of dynamics calculations, output file for later replay on a vec- 
tor graphics system, output file for parameter plots, start time, stop 
time, and processing stepsize. 


SUBROUTINE REQOPT 



INPUT MOD CRTEGORV IS 2 


PROMPT FOB MOCJXFXCRTXON TO PRINT OUTPUT LEVEL 
PRINT OPTION IS DESIRED ~ 

phoh^t fctTfbj nrh e of ppont output | iT Ti" 


(NULL) 


INPUT MOO 
CHT gQORV IS 3 


OPEN RLL 
OESIREO 
PILES- SET 
. FLAGS 


PROMPT TOR MOO. TO TORQUE OUTPUT DR TP OPTION 
TORQUE OUTPUT FILE DESIRED 
PROMPT FOR PILE MPMC OF TORQUE OUTPUT 1 7TT] IT" 


(NULL) 




CO NTRO L- VIS IhPUT RCC-Ve3_-THETS INPUT FILE 


(NULL) 


PROMPT FOR - RVT FH-H NSMg 

PROMPT FOR MOO. TO DVNRMICg" COMPUTRTION8 OPTION 
PROMPT FOR MOO. TO SIM. OUTPUT FILE OPTION 
Tv . SOF OUTPUT "FILE IS DESIRED 


1 REflO 
STRRT 
TIME FROM 
FILE 


,RERO XNITXRL. 
1 UT. 



. PLT OUTPUT FILE IS DESIRED 



ror NfMe or outtut . fi_t nui 


(NULL) 


PROMPT FOR TIME 9TEF FOR . FLT FILE | 

PROMPT FOR REQ. SIM. STRRT TIME 
PROMPT FOR REQ. SIM. STOP TIME 
PROMPT FOR PROCESSING STEP SIZE 


WRITE /IOPTBK/. 
/TIMES K/. 
/FLNflHBK/ COMMONS 

CLOSE AND 
SRVE REQ. 
OPT. FILE 


■ 


\ j i : • . n . 'U •;*» i 5P «•** 

OF FOOR QUALITY 
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2.2.3 PRTARM 



Subroutine PRTARM is called form either REQUIR or RESPON when the flag 
for printed output of that analysis is set. This routine prints a de- 
scription of the manipulator system that includes the following vari- 
ables: current arm number and number of joints per arm, type and mass 

of each joint, initial angular positions and velocities of each joint, 
joint travel and rate limits, joint/link centroid locations, joint lo- 
cation relative to previous joint, inertia matrix for each joint, orien- 
tation matrix for each joint relative to previous joint, span of the 
whole system, and the acceleration attributable to gravity. 


SUBROUTINE PRTRRM 


■ 

WRITE CURRENT ARM NUMBER AND THE 
NUMBER OF JOINTS PER RRM 


WRITE 

THE TYPE AND MASS OF ERCH JOINT 


CONVERT DATA TO BE WRITTEN FROM 
INTERNAL TO INPUT/OUTPUT UNITS 


WRITE 

JOINT INITIAL ANGULAR POSITIONS 
RND VELOCITIES 


WRITE 

JOINT TRAVEL RND RRTE LIMITS 


WRITE 

JOINT/LINK CENTROID LOCATIONS 


WRITE JOINT LOCATIONS RELATIVE TO 

PREVIOUS JOINT 

1 

WRITE 

INERTIA MATRICES FOR JOINT/LINK 
COMBINATIONS 

• 

WRITE ORIENTATION MRTRICES FOR ERCH 
JOINT RELATIVE TO PREVIOUS JOINT 

DO 

UNTIL. 

KARM - NUMBER OF ARMS IN THE SYSTEM 

WRITE TOTRL SYSTEM SPRN 

WRITE 

RCCELERRTION DUE TO GRRVITY 

RETURN 

END 
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2.2.4 


GRAFIX 


Subroutine GRAFIX provides the motion graphics capability in the force/ 
torque requirements analysis tools and postprocessing functions. GRAFIX 
displays the environment, load and robotic system motion within the en- 
vironment. If IFLAG-1, the graphics system is initialized and displayed 
in the initial condition; if IFLAG-2, the display is updated to the cur- 
rent time step condition; if IFLAG=3 , the motion is complete and the 
graphics are terminated* 


SUBROUTINE GRAFIX 



IDO FOR CP CH LORO OBJECT 


SET TRPNS. AND ROT. BBSEO ON RLPLQC RhO RLPOR MATRICES 


loo 

FOR ERCH COMPONENT IN 


— — 





i u — n i ' iJ 1 

EEESgSOE i i 1 



COUNTER FOR LOST LOROS 

OBJECT RRRRY LOCRTION 

USED i 


|PO FOR BASE. EACH LINK. PND TOOL. OF CURRENT RRM 


SET TRRNS. PND ROT. HPTRICES BASED ON PQ3 RND WOT MPTRICgS 


SET NUMBER OF COMPONENTS IN LINK PRRRMETER 


|DO FOR ERCH COMPONENT XN LINK 


I SET GRAPHICS FLRGS FOR SEQUEN T! BL PND RLTERNPTING P T _S^__ 
CR1—L. D3QRTR TO DISPLAY COMPONENT ] 


SET COUNTER FOR LAST RRM OBJECT ARRAY LOCRTTON USED 


r * i a*JT\ OFPLflCE SEGMENT 



ur ‘ G£ ^al page m 

0F p 00R QUALITY 
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2.2.5 CNTRLR 


Subroutine CNTRLR is called from REQUIR to obtain the angular position, 
velocity, and acceleration for each joint of each arm at each processing 
time step. If the variable IDATA was set to 1 earlier, the data are ob- 
tained by reading an existing file that contains just those data. If 
IDATA equals 2, the values are calculated from the motion profiles. 
Subroutine PCNTRL is called for the position control calculations and 
RCNTRL is called for the rate control calculations. IDATA equal to 3 
allows the system motion to be controlled by dials on the Evans and 
Sutherland . 


SUBROUTINE CNTRLR 


Vs. 



■— — — I .1 ■■ i ■ ill Hill 


mi 

■ 



re 



IORTR . EQ. 2 






H— H 

















Hjlf 


— J 




L— J 

Sgl wm 











i 


LlJ 

cc 





— i 


CD 

9 







z£ S 



wlzzL xk irM . m MW 



' 


i . * 










a 












etc 

■■■ 

TIME 

. EQ. STRRT 





1 






! — 1 „ 

* 

READ INITIAL TH 





z err 

m 

CALL SETUP2 

CNUL-LD 



3D CO 

1 

INITIALIZE 

VALUES 




21} 

CD 1 1 
CD —2 

■ 

TIME . 

GT. STOPTIM 




1 

ZD 




RETURN 

| CNUL.L-3 




1 => 

■ 

CALCULATE NEW TH 



S Cy 

□o 

UNTIL. KARM=NRRM 







CALL ESCNTRL 










or 

■ 

>v 

IRRTE - EQ. 1 






i — 

1 

CALL RNCTRL 

CRLL RflTEPRO 


1.x J 

or 


H CALL PCNTRL- 







OR.'T"' , 'V <- - - 

0F fo : .:'r 
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2.2.6 SPRGINC 


Subroutine SPRGINC is called from REQUIR to set the variables used when 
the end-effector is to have compliance associated with it. The vari- 
ables set includes spring reference position, orientation and the spring 
constant. 



SUBROUTINE SPRGINC 

(TEMPORARY) 

SET SPRING VARIABLES' SPRP CPOS) . 
SPROR (ORIENTATION) . RND 
SPRK CSTIFFNESS M 

1 

N -= NU CKRRM) 2 

1 

POS Cl. N. KRRM) . LE. . 68872 y' 

\ . RND. POS C3. N. KRRM) . GE. / 

T \ . 68872 / F 

1 

ISPR CKRRM) - 2 

CNULL) 


DO UNTIL KRRM . EQ. NUMBER OF ARMS 
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2.2.7 CHKLMT 



CHKLMT checks joint displacement and rate limits during requirements 
analysis. It does not modify any values but prints a warning to the 
terminal if any limits are exceeded. 



SUBROUTINE CHKLMT 


□O FOR ERCH ARM IN SYSTEM 


DO FOR ERCH JOINT IN RRM 


\ DISPLRCEMENT EXCEEDS 

T X MINIMUM OR MRXIMUM VRLUE /p 

TYPE WARNING TO 
TERMINAL WITH 
PERTINENT DRTR 

CNULLD 

N. RRTE EXCEEDS MINIMUM OR 

t \ n ^ MRXIMUM VRLUE / p 

TYPE WARNING TO 
TERMINAL WITH 
PERTINENT DRTR 

CNULL3 


JOINT RRTE EQUATIONS COULD 
NOT BE SOLVED 


TYRE WRRNXNG TO 
TERMINAL WITH PERTINENT 
ORTR 


CNULL3 
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2.2.8 DYNAM 


Subroutine DYNAM Is called from REQUIR to compute the manipulator sys- 
tem dynamics at each processing time step by calling the SETUP, CABSM, 
FORCE, TORQUE, and ACTORQ subroutines. 


SUBROUTINE DYNRM 


CRL_L_ SETUP 
IN 

TO FIND RLL POSITIONS 
WORLD COORDINRTES 

CRL_L_ CRBSM TO FIND RBSOLUTE VEL. 
RND RCCEL. OF RLL LINKS 

CRLL 

FORCE 

TO 

FIND UOINT 
FORCES 

RERCTION 

CRLL 

TORQUE 

TO 

FIND UOINT 
TORQUES 

RERCTION 

CRLL 

RCTORQ 

TO 

FIND UOINT 
TORQUES 

RCTURTOR 

RETURN 

END 
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2 . 2.9 


VOLTAGE 


(Not implemented yet.) 



2.2.10 OUTREQ 

Subroutine OUTREQ is called from REQUIR to write output data to files 
requested by the user. The files the user may elect to have data 
written to are: 

1) Run time output data file; 

2) Data file for subsequent replay of motion on a vector graphics 
system; 

3) Actuator torque data file; 

4) Run time data file for subsequent plotting. 


SUBROUTINE OUTREQ 


^ FIRST CRLL TO SUBROUTINE 

SET TIME FLRGS 

CNUL-I— ) 

IPRINT . LE. 2 

\C0RRECT TIME TO WRITE DRTR y£ 

(NULL) 

CflLL REOPRT TO WRITE 
PRINTED OUTPUT PILE 

CNULL3 

^ ISIMO . EQ. 1 /£ 

CORRECT TIME TO WRITE DRTR y£ 

(NULL) 

call Rpcmnp to write 

SIMULATION OUTPUT PILE 

(NULL) 

ITORQ . EQ- 1 

^CORRECT TIME TO WRITE DRTR y£ 

CNUl_L_) 

CALL REQTRQ TO WRITE 
TORQUE OUTPUT PILE 

(NULL) 

IPLOT . EQ- 1 

CORRECT TIME TO WRITE DRTR 

(NULL) 

CALL REQPLT TO WRITE 
PLOT DRTR PILE 

(NULL) 

RETURN 

END 
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2.2.11 ESPAUS 


Routine ESPAUS Is responsible for polling the status of the E&S function 
keys to determine the on/off status of the devices switch for playback 
motion cessation. A light indicator in the function key is used to in- 
form the user of the key status; when lighted, the perspective viewing 
is in operation. 



SUBROUTINE 

ESPAUS 


STATUS FUNCTION KEYS CEXTENDED 
SWITCHES! EVENT QUEUE 

T 

USER FUNCTION KEY FOR 
TERMINATING IS ACTIVATED 


ISTOP OUTPUT 
FLAG - O 

i 

(NULL) 

% 

T 

USER FUNCTION KEY FOR 
. CESSATION OF MOTION IS 
\ ACTIVATED 


ISTOP OUTPUT 
FLAG = 1 

(NULL) 

RETURN 

END 
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2.2.12 ENDREQ 


Subroutine ENDREQ closes any files opened during running of the require- 
ments analysis portion of ROBSIM. 


SUBROUTINE ENDREQ 
FILE 3 IS OPEN 


CLOSE LU3 


FILE 6 IS OPEN 


CLOSE LU6 


FII-E 1 3 IS OPEN 


CLOSE LU 1 3 


FILE 14 IS OPEN 


CLOSE LU14 


FILE 1 6 IS OPEN 


CLOSE LU 1 6 


FIL-E IV CKRRM3 IS OPEN 


CLOSE LU 1 V CKRRM3 


DO UNTIL KRRM . EQ. NUMBER OF RRMS 


RETURN 



(NULL) 


(NULL) 


(NULL) 


(NULL) 


(NULL) 


CNUUL3 




2.2.13 SIMOPT 


ORIGINAL PAGE ® 
OF POOR QUALiTY 



Subroutine SIMOPT interactively prompts the user for the program start 
time, stop time, processing time step, and several flags for control of 
output and the selection of some computational capabilities. Among 
these output options is a simulation output file that contains the data 
required by the postprocessing function for further study. The user 
also specifies the time frequency of the output of data to the file. 

The user is also allowed to request printed output during the analysis 
tools function execution. 


The content and format of the data to be printed are provided for within 
each of the analysis tools. The flag set within SIMOPT is used only to 
turn the print routines on. The time frequency of the printed output 
is also specified. Other options are for generation of an acceleration- 
velocity-theta file and/or a plot output data file that may be plotted 
with the ROBSIM postprocessing plot utility with their associated out- 
put time steps. The user may also request use of a torque input file 
or a control option to read a hardware input voltage file for computa- 
tional capabilities. 
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2.2.14 INITCO 


Subroutine INITCO prompts the user for the initial joint position (TH) 
and velocity (THD) of each joint of each arm. 


SUBROUTINE INITCO 


1 

1 

PROMPT USER FOR INITIAL JOINT 
POSITION — TN CN. KRRM) 

i 


PROMPT USER 

FOR INITIAL JOINT 


1 

VELOCITY — THD CN- KRRM) 

1 

DO UNTIL N . EQ. 

NUMBER OF JOINTS 

DC 

UNTIL KRRM . EQ. 

NUMBER OF RRMS 

RETURN 

END 
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2.2.15 DEFCNST 

DEFCNST reads a file containing the information needed to define a con- 
straint (either planar or peg-in-hole type) on the end-effector motion 
during dynamic simulation of the arm response. The user specifies the 
name of the constraint file in response to interactive prompts. 



SUBROUTINE DEFCNST 


QUERY WHETHER USER WRNTS TO INCLUDE CONSTRAINT 


CONSTRAINT DESIRED 


KRRM = 1 


PROMPT FOR FILENAME OF CONSTRAINT FILE 


OPEN CONSTRAINT FILE 


READ TOOL REFERENCE POINT LOCATION 


READ NUMBER OF PLANAR CONSTRAINTS 


DO FOR EACH PLANAR CONSTRAINT 


READ THE * COORDINATES DEFINING THE PLANE 


READ 

NUMBER OF 

PEG-IN-HOLE CONSTRRINTS 

DO FOR EACH PEG— IN-HOLE CONSTRAINT 


READ 

HOLE LOCATION 


READ DIRECTION OF HOLE AXIS 

■ 


READ 

DEPTH OF HOLE 

■ 


READ 

RADIUS OF HOLE 


READ 

FRICTION 

COEFFICIENT FOR HOLE 

■ 


CLOSE CONSTRAINT FILE 


HOLE CONSTRAINT INCLUDED 


ACTIVATE HOLE CONSTRAINT 


□CFXNC HOLE CONSTRAINT INRCTIVE 
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2.2.16 PIDINIT 


Subroutine PIDINT is called from RESPON to initialize variables used in 
the program's control algorithms. POSSENS is called first to determine 
the actual joint positions. Initial values for some control variables 
are set. The user is then asked to supply system gains for the methods 
of control that will be used during program execution. These gains may 
be supplied by either reading in a file of existing gains or by the user 
interactively inputting the gains. 


SUBROUTINE PIDINIT 


CALL POSSENS TO 

OBTAIN STH 



INITIALIZE STHOo STWDO. OLDSTM. ERRINT. 

AMPVE 

FERRINT. SERRINT. AND 


USING RID 

CONTROL 


Sf 

READING GRINS FROM 

R 

FILE 



PROMPT USER FOR FILENAME 

PROMPT USER 
TO INPUT 
GRIN DPTP 

CNULLD 


READ GAINS FROM FILE 




USING FORCE/TORQUE CONTROL 


Sf 

^ READING GRINS FROM 

R 

FILE 

— 


PROMPT USER FOR FILENAME 

PROMPT USER 
TO INPUT 
GRIN DRTR 



READ GAINS FROM FILE 

' 


USING ACTIVE COMPLIANCE CONTROL 


/f 

\ READING GRINS FROM R / 

T \ FILE /F 



PROMPT USER FOR 
FILENAME 

PROMPT 
USER' TO 



READ GRINS FROM 
FILE 

INPUT 
GRIN DRTR 



RETURN 

END 
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2.2.17 OUTSIM 


Subroutine OUTSIM is called from RESPON to write the appropriate output 
data to the different types of files requested by the user. Types of 
output files available are: 

1) File of run time data for subsequent tabular printout; 

2) File of joint positions, velocities and accelerations as functions 
of time; 

3) Data file for later motion replay on vector graphics machine; 

4) File of data for subsequent x-y plotting. 


SUBROUTINE OUTSIM 



FIRST CALL TO SUBROUTINE 


SET 

TIME FLRGS TO STRRT TIME 


(NULL) 


HR 

IPRINT . LE. : 

2 





CORRECT TIME TO WRITE DATA 



f N1 II 1 1 ! 


Ml. MMIT TB Mill IMMV MPUT AA | CNULLD | 



DO UNTIL KRRM . EQ. NUMBER 

OF 

RRMS 



IDflTA . EQ. 

L 


^ 


CORRECT TIME TO WRITE DRTR 




WRITE CURRENT TIME TO OUTPUT FILE 



(NULL) 

|URXTE m TWO. THDO TO OUTPUT FILE 

(NULL] 

DO 

UNTIL. KRRM . EQ. NUMBER OF ARMS 




K 

ISIMO - EQ- 

1 



r 


. : -j : — : i— , 

WRITE CURRENT TIME TO OUTPUT FIl_E 

^ -r- 

(NULL) 

I 

| WRITE TH. ILO TO OUTPUT FILE 


DO UNTIL KRRM . EQ. NUMBER OF RRMS 


^ IPLOT . EQ. 1 

^CORRECT TXME TO UIRXTE DRTR 

(NULL) 

CRLL SIMPLT TO WRITE fKJl II i ^ 

PLOT OUTPUT FILE U>IL-IL-L_J 

RETURN 

END 
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2.2.18 CNTRSIG 


Subroutine CNTRSIG is called from the routine REQUIR. Joint variables 
are stored in dummy variables and CNTRLR is called to calculate joint 
angular reference positions and velocities. The end— effector position 
error is calculated and ORERR is called to determine the orientation 
error. If force/torque control is being used, subroutines FORTOR and 
FORREF are called to calculate joint reference positions and reference 
forces and torques. If active compliance control is being used, sub- 
routine CMPCTRL is called to calculate amplifier input voltages. 


SUBROUTINE CNTRSIG 


SRVE VRLUES OF TH. THD. POS. ROT. IDRTR. 

RND TIME IN DUMMY VRRIRBLES 


CRI_L_ CNTRLR TO CALCULATE REFTHT RND 

REFTHDT 


RETURN VALUES STORED IN DUMMY VAIRBLES 


END EFFECTOR POSITION ERROR. 

EPOS - POS 


EPSERR 


CALL ORERR TO FIND THE END EFFECTOR 

ORIENTATION ERROR 


T 


USING FOREC/TORQUE CONTROL 


CALL FORTOR TO CRLCULRTE 
REFERENCE JOINT POSITIONS 


CRl_l_ FORREF TO CRLCULRTE 
REFERENCE FORCES RND TORQUES 


CNUL-L3 


USING RCTIVE COMPLIANCE CONTROL 


CALL CMPCTRL TO CRLCULRTE MOTOR 
AMPLIFIER INPUT VOLTRGES 


CNULU 


RETURN 


:nd 
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2.2.19 CONTROL 



Subroutine CONTROL is called from RESPON at every processing time step. 
If a feedback control law is to be used, POSSENS is called to get the 
actual joint data and PIDCON is called to get actuator voltages for PID 
control. If force/torque control is being used, PIDFOR is also called 
to get actuator voltages caused by the force-controlled components. 



RETURN 


END 
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2.2.20 SETCNST 


SETCNST checks planar constraints to see If they are violated or need 
to be activated. If the current velocity violates the constraint, the 
velocity impulse to satisfy the constraint is evaluated. 



SUBROUTINE SETCNST 


DO FOR ERCH RRM 


NUMBER OF PLRNRR CONSTRRINTS 
GRERTER THRN ZERO 


EVALUATE DISTANCE TO CONSTRAINT 

PLANE 


DISTANCE LESS THRN ZERO 


PRINT ■CONSTRAINT VIOLATED* 
WARNING TO TERMINAL 


CNUL_L_D 


T 


DISTRNCE LESS THRN TOLERANCE 


SET UP FLAGS TO RCTIVRTE 
CONSTRAINT 

EVALUATE VELOCITY OF POINT 
TOWARD CONSTRAINT PLANE 

VELOCITY GRERTER THRN 

EERO 

/ 

/F 

EVALUATE JOINT RRTE IMPULSE 
TO MAKE VELOCITY ZERO 




PRINT ^VELOCITY IMPULSE! 
WARNING TO TERMINAL 




UPDATE JOINT VELOCITIES 







B-121 


(NULL) 














2.2.21 DERIV 


! 


i 

! 


DERIV is used during response simulation to interface between INTGRT and 
the dynamics module NLINK. This routine puts the state vector Z into 
the appropriate common variables, calls NLINK and puts the results from 
the common variable THDD into ZD. 


SUBROUTINE DERIV 

SET TEMP. TIME TO TIME 


DO FOR ERCH RRM 


DO FOR 

ERCH 

JT. IN RRM 


! 

SET RNG. 

POS. 

• VEL. PND RCC, 

FROM STATE VECTOR 

■ 

SET DERXV. 

STATE VECTOR 

FROM 

RNG. RCC. 

■ 


SET 

DIMENSION OF 

STRTE 

VECTOR 


CRLL NLINK TO SOLVE FOR JT. RCC. 


Cnt_l_ SIMLMT TO CHECK JT. DISPLACEMENTS AND RATE LIMITS 

SET TIME TO TEMP. TIME 
DO FOR ERCH RRM 


DO FOR ERCH JT. OF RRM 


SET STRTE DERIV. 

VECTOR FROM RNG. RCC. 


\ JT. POS. /RATES MERE 
T\ 

MODIFIED TO KEEP WITHIN LIMITS 
THEN 

/f 

SET STRTE DERIV. VECTOR FROM RNG. VEL. 



SET STRTE VECTOR 

FROM RNG. VEL. 

(NULL] 


SET STRTE VECTOR 

FROM RNG. POS. 

























2.2.22 INTGRT 


Subroutine INTGRT is called from RESPON and uses a fourth-order Runge- 
Kutta algorithm to integrate a state vector Z. State derivatives are 
computed by the subroutine DERIV. 



SUBROUTINE INTGRT 

SET T TO XNXTXRL TIME FOR INTESRflTXON 
SET HDT TO TIME STEP/2. 


DO FOR ERCH COMPONENT IN STRTE 







Wm 


Cfit_L STOPFR TO STOP MOTION DUE TO DRY FRICTION 


SET T TO INITIRL. TIMEf HOT 

COLL DERIV TO CALCULATE STRTE PERIVRTIVES 


DO FOR ERCH COMPONENT IN STRTE 





9HH 


w 


CRL.L. STOPFR 


CALL DERIV 


DO FOR ERCH COMPONENT IN STRTE 



■ 


■ 




CALL STOPFR 


SET T TO INITIRL TIME TIME STEP 

CALL DERIV 

DO FOR ERCH COMPONENT IN STRTE 

SET DELZ TO LAST DELZfSTBTE DERIV. «HDT 

SET STRTE VECTOR TO SRVE STRTE VECTOR»OELZ/3. 

CALL STOPFR 

CALL DERIV 

RETURN 

END 
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2.2.23 ENDSIM 


Subroutine ENDSIM closes any files opened during execution of the re- 
sponse simulation portion of ROBSIM. 



END 
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2.3.1 SEGINIT 


Subroutine SEGINIT is called from CNTRLR at the beginning of each new 
motion profile segment. If the segment is to define motion of the 
manipulator, the coefficients of the polynomials defining the motion 
rates or the desired positions and orientations are read from the mo- 
tion history file. If it is a nonmotion segment, the appropriate sub- 
routines are called or variables are defined to ensure these actions 
are carried out. If position control is specified, the motion deltas 
for the current time segment are calculated . 


SUBROUTINE SEGINIT 
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2.3.2 GRASP 


Subroutine GRASP is called from SEGINIT when the option flag IOP equals 
1 (this denotes that the arm is to grasp a load object). The subroutine 
first checks to make sure the arm is not already holding an object and 
that the desired object is not being held by another arm. The location 
and orientation of the load object are then defined with respect to the 
end-effector coordinate system. This ensures that the object's loca- 
tion and orientation with respect to the world coordinate system will 
be updated correctly during a move and that the display shows the object 
moving with the arm. The end-effector mass properties are modified to 
include the load object to ensure the correct system response. 


SUBROUTINE GRRSP 


DESIGNATED RRM NOT CURRENTLY 
HOLDING RN OBJECT 


DESIRED OBJECT NOT HELD BY 
RNOTHER RRM 


CRLL MRTMPY TO GET OBJECT LOCRL 
CG VECTOR IN WORLD COOR SYSTEM 


CRLCULRTE TOOL ORIGIN TO OBJECT 
CG VECTOR IN WORLD COOR 


CRLL MRTMPY TO GET ABOVE 
VECTOR IN TOOL COOR SYSTEM 


CALL MRTMPY TO PUT TOOL ORIGIN TO 
OBJECT ORIGIN VECTOR IN TOOL COOR SYST 


ac 

Li-l 

co 


e_o 

CE 

CO 

CO 


CO 

CO 


c_o 

ac 

CO 

CO 


Ut 1 th(|viIINt UOUCL 1 1 l_J 1 DLH 

TRRNSFORMRTION MRTRIX 

1 i 1 

UJ 
i — i 

CRLL. MRTMPY TO GET OBJECT LOCRL INERTIA 
MRTRIX IN TOOL COOR SYSTEM 

h— 
1 — 1 
cn 


CRLL ADDMAS TO COMBINE OBJECT 
RND TOOL MRSS PROPERTIES 



RETURN 

END 


B-126 















2.3.3 RELEAS 


Subroutine RELEAS is called from SEGINIT when the arm is to let go of a 
load object. The routine first makes sure the load object to be re- 
leased is being held by the current arm. If it is being held, the end- 
effector mass properties are reset to the values held before the object 
was picked up and MATMPY is called to obtain the location and orienta- 
tion of the load object with respect to the world. If the object is not 
being held, a message is displayed to the user. 
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2.3.4 ESCNTRL 




ESCNTRL allows control of system motion through use of the Evans and 
Sutherland extended switches (function keys) and loads the coefficients 
of the polynomial describing the motion, PCOEF. Options include indi- 
vidual joint control or end-effector control. For end-effector control, 
either the base coordinates or tool coordinates may be used as the re- 
ference frame. Also, the controlled motion may be either translation 
or rotation. The manipulator motion is always rate-controlled rather 
than joint-controlled. The user may select which arm and joint is to 
move . 


SUBROUTINE ESCNTRL 


EERO MOTION PROFILE COEFFICIENTS FOR ERCM PRRM. OF LINKS. PER RRM 
STATUS FUNCTION KEYS (EXTENDED SWITCHES! EVENT QUEUE 

OO NN1LE FUNCTION KEYS RCTXV R T EL) FOR I N TE RAC T I VE CONT R O L . OF UTS. 

POLE STRTUS OF INDIVIDUAL FUNCTION KEYS 
SET PARAMETER FOR UOXNT OR END-EFFECTOR CONTROL 
SET PARAMETER SPECIFYING POSITION OR RATE CONTROL 


SET PARAMETER FOR TRANSNATIONAL OR ROTATIONAL JT. MOTION 


END-EFFECTOR CONTROL 


TRflNSLRTIONRL / 

JT. MOTION /f 


CONVERT 

30. 

DEG/SEC 


V I I 

TO RRD/SEC 
SET MOTION 
PROFILE 

SET MOTION SET MOTION COEFF. . 6 

PROFILE COEFF. . PROFILE COEFF. . VECTOR 

ELEMENTS Is 2s 3s ELEMENTS <4. S. 6s ELEMENTS. TO 


SET VELOCITY 
VRLUE TO SET 
VRLUE OF 12. 
INCHES/SEC 


CONVERT 30. 
DEG/SEC VELOCITY 
TO RRD/SEC 


TO SET VELOCITY [ TO SET VELOCITY |SET VELOCITY 
STOP TIME FOR SIMULATION - CURRENT TIME - STEP SIZE 

RETURN 

END 
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2 . 3.5 


POSSPJT 


POSSPJT is the executive routine that calls handling routines for find- 
ing the position of special joints within the manipulator. Currently 
only one type of special joint can be included. 


SUBROUTINE POSSPUT 


DO FOR ERCH RRM 


X 

T 

. SPECIAL JOINTS RRE 

X INCLUDED 

X 

F 

□ O 

FOR ERCH SPECIRL UOINT 



\ 

T 

v TYPE EQUAL // 

X GORDY UOINT / 

\ / F 


i 


CALL 

POSGRDJT 

CNULL3 

1 












2.3.6 RCNTRL 


Subroutine RCNTRL is called from CNTRLR when rate control of the joints 
or end-effector is specified. If joint rate control was chosen, the 
joint rates THD are calculated from their polynomial definitions and the 
accelerations THDD by finite difference methods. For end-effector rate 
control, the defining polynomials are evaluated for the current time. 
JACOB and SLVLIN2 are then called to transform these end -effector rates 
to individual joint rates. Accelerations (of each joint) are again cal- 
culated using finite difference methods. 


SUBROUTINE RCNTRL 


^ UNTTUL . EQ. 1 


LCULflTE 

(JOINT 

KITES) 

CRLC ROOT RND OMDOT (END 
EFF. RRTES3 

^ JNTTUL - EQ. 2 

5 jEE 

PUT RDOT 
RND OMDOT 
XN VECTOR 
ENDVEL 

TRANSFORM ROOT RND OMOOT 
OT MORLO COOR SYSTEM 

1 i 

H— l-H 

I s ! 

PUT 11 UU ™vcctor EK5P M ™ 

CRLL JACOB TO CRLC 
JRCOBXRN 

CRLL SLVLXN2 TO SOLVE FOR 
JOINT RRTES 
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2.3.7 RATEPRO 


RATEPRO is called from subroutine CNTRLR when position control of the 
manipulator is desired. The time allowed for the move is divided into 
six equal portions. The first portion is defined to be constant accel- 
eration. The next four are constant velocity. The last is constant 
deceleration equal in magnitude to the first portion. The distance 
traveled in the whole time is set to 1 and the appropriate distance 
traveled, velocity and acceleration for each portion are calculated. 


SUBROUTINE RATEPRO 


DELTIM — TIMSEG/6. O 

RCCEL = 1. O/ CS. 0*DELTIM**2i 

VELOC = flCCEL^DELTIM 

T . LT. 
OELTIM 

DOCnSE 

T - GE. DELTIM 
. AND* T . LT. 

CS- 0~DEI_TXM3 

T . GE. 

CS. O^DELTIMJ 


R - 

. 5*RCCEL**2 

R - 

. 5#ACCEL#OELTIM**2 

P - 

-K 5-»ACCEL*DELTIM<m>2 

R - 

R-»-VEL.OC~ CT-S. 0-*D«_TXMJ 

R - 

R^VELOC* CT-OEL-TXMJ 

RD — 

R - 



8*ACCBL* TT— TT- 0*0ELTZM1 


RCCEL*T 

RD = VELOC 


z 

RO - 

VKLOC- CACCXL* CT-«« 0~0«L.TTW1 1 


RDD = 
RCCEL 

RDD = 0.0 

RDD = —RCCEL 

RETURN 

END 
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2.3.8 PCNTRL 


PCNTRL is called from CNTRLR when position control of the manipulator is 
to be used. Joint position control uses the segment rate profile de- 
fined by subroutine RATEPRO to calculate the joint positions, veloci- 
ties, and accelerations. End-effector position control uses the same 
rate profile to get the end-effector rates. JTPOS is then called to get 
joint positions, and JACOB and SLVLIN2 are called to get the joint velo- 
cities. Joint accelerations are calculated by finite difference me- 
thods . 


SUBROUTINE PCNTRL 


JNTTUL . EQ. 1 

TH - 

RR*DELTH+SEGTH 

EPOS = RR*DELPOS+SEGPQS 

CRLCULRTE THE ANGLE OF 
ROTATION 

CPU-U MRTMPY TO FIND THE 
ORIENTRTION TRRNSFORMRTION MATRIX 

THD «= 
RD*DELTH 

\ USING FORCE/TORQUE OR / 

T\ COMPLIANCE CONTROL /F 

SET POSREF fK „ .I . , 

AND ORREF CNULLD 

THDD = 
RDD*DELTH 

CALL UTPOS TO GET UOINT 
POSITIONS 

CALCULATE ENDVEL. END 
EFFECTOR VELOCITY 

CALL SETUP 
TO CRLC ALL 
POSITIONS 
IN WORLD 
COORDINRTES 

CALL JACOB AND SLVLIN2 TO 
GET UOINT VELOCITIES 

THDD — 

CTHD— OLDTHD) /STPPRO 

RETURN 

END 
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2.3.9 CABSM 


CABSM uses a recursive technique to compute the absolute angular and 
translational velocity and acceleration of each joint/link combination 
in the system. 



SUBROUTINE CRBSM 


DO FOR ERCH RRM IN SYSTEM 


INITIALIZE W. V. RL_» AND R WITH RNG. AND 
TRRNS. VEl_. RND RCC. OF BRSE 


DO FOR ERCH LINK 


V = V W CROSS HIJ CVECTOR FROM 

PREVIOUS UOINT TO CURRENT JOINT) 

R — R tW CROSS CW CROSS HIJ) ) 

CRU CROSS HIJ) 

VJ - THETR-DOT TIMES DIRECTION 
VECTOR FOR UOINT RXIS 

AJ = THETR-DOUBLE-DOT TIMES 
DIRECTION VECTOR FOR UOINT RXIS 

WCVJ = W 

CROSS VU 


\ UOINT IS 

T \ 

REVOLUTE 

^ 

/f^ 

WJ — W H- VU 

v = V VU 

RL = AL -+- AJ 
WCVJ 

R = R -h RU 
2-k-WCVU 
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2.3.10 FORCE 


Subroutine FORCE is called from DYNAM to calculate the force exerted on 
each joint. The force at the end-effector is determined first. PTACC 
is called to find link centroid accelerations, and the forces caused by 
these accelerations are calculated and added to the end-effector forces 
to find the force at each joint. 


SUBROUTINE FORCE 


INITIALIZE FEND RND TEND TO ZERO 

VECTORS 

\ END EFFECTOR IS N 

T\ SPRH 

lODELED RS R / 

[NG / \ F 

CPLL SRRGFOR TO FIND FORCES 
RND TORQUES WHEN END EFFECTOR 
HRS CC3MPLZRNCE 

CNULL3 

CONSTRRINT INCLUDED IN SYSTEM 

CRL-L CNSTFOR TO CRLCULRTE 
CONSTRRINT FORCES 

CNULL3 


CALL PTRCC TO FIND LINK 
CENTROID RCCELERATIONS 

CALCULATE FORCE DUE TO ACCELERATION 

OF THE CENTROID 

RDD TO END EFFECTOR FORCE TO 
OBTRIN FORCE RT JOINT 

DO 

UNTIL N MOVES FROM END BRCK TO BASE 


DO UNTIL KRRM . EQ. NUMBER OF RRMS 

RETURN 

END 
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2.3.11 TORQUE 


Subroutine TORQUE is called from DYNAM to calculate individual joint 
torques. The torques at the end-effector are determined first. Tor- 
ques at the preceding joint are then calculated by adding the torques 
attributable to link inertias and centroid forces to the end-effector 
torques. The routine works back toward the base of the manipulator, 
adding the torques caused by inertias and centroid forces to the cumu 
lative torques thus far to obtain the current joint torques. 


SUBROUTINE TORQUE 


INITIALIZE T 

TO TEND 

■ 

\ N DENOTES END 

T \ 

EFFECTOR / 


DEFINE THE TORQUE 
TERM FOR THE END 
EFFECTOR 

CNULL3 


CRL_L_ MRTMPY RND CRPD TO COMPUTE 
INERTIA TORQUES 


CR!_t_ CRPD TO COMPUTE TORQUES 
DUE TO CENTROID FORCES 

DC 

TC 

UNTIL N MOVES FROM 
BRSE 

END BRCK 


DO UNTIL. KRRM . EQ. NUMBER OF RRMS 


RETURN 


END 
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2.3.12 ACTORQ 



Subroutine ACTORQ calculates actuator drive torques for each joint. It 
Is called from REQUIR when running requirements analysis and from NLINK 
when running response simulation. This routine first solves for the 
free axis of each joint and the component of joint reaction torque about 
this axis. The torque needed to overcome inertia and viscous and dry 
friction are added to the joint reaction torques to obtain a total actu- 
ator drive torque for each joint. 


SUBROUTINE RCTORQ 


■ 


INCREMENT 

KRRM 



j;;;: 


INCREMENT 

N 




I 

SET NRXIS TO RXIS OF 
BRSED ON JOINT 

ROTRTION 

TYPE 



1 

I 

\ NRXIS . 

T \ 

NE. 

O 

z 

4 

1 


SOLVE FOR TERM OF 
RCTURTOR TORQUE DUE 
TO LINK N OF REVOLUTE 
JOINT 

SOLVE FOR TERM OF 
RCTURTOR TORQUE DUE 
TO LINK N OF SLIDING 
JOINT 

£ 

1 

RDD LINK N MOTOR INERTIA RND 
FRICTION TERMS TO RCTURTOR 

TORQUE 

jf 

DO UNTIL N — NUMBER OF JOINTS IN 
KRRM 

DO UNTIL KRRM = NRRM 

RETURN 

END 
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2.3.13 REQPRT 


Subroutine REQPRT is called from OUTREQ to write run data to an output 
file if this option was requested by the user. Data written to this 
file includes time, angular position, velocity and acceleration, trans- 
lational position, velocity and acceleration, and joint force and torque 
vectors . 



SUBROUTINE REQPRT 


DO FOR EACH ROBOTIC RRM 


WRITE TIME. RRM NUMBER TO PRINT FILE 


CONVERT THETR VALUES TO OUTPUT UNITS 


WRITE RNG. POSITION. VEL. . RCC. . 
RCT. TOR. FOR EACH JT. TO FILE 


DO FOR ERCH 

UT. 

AND 

END-EFF. 


WRITE TRANS. 

POS. * 

VEL. 

- ACC. TO FILE 


x 

NOT 

END- 

-EFF. 



WRITE ABSOLUTE RNG. VEL. 
RNG. RCC. 


WRITE ROT. MBIT. FROM »JT. TO INERTIBL. INERTIR 
MRT. FOR LINK IN WORLD 


WRITE JT. FORCE VECT. . FORCE 
VECT, RT JT. /LINK CENTROID 

WRITE UT. TORQUE VECTOR 

RETURN 

END 
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2.3.14 REQSOF 


REQSOF is called from OUTREQ to write a simulation playback file if this 
option was requested by the user. The simulation playback file contains 
joint angular positions as a function of time and is used to replay the 
motion that occurred during a requirements analysis run without doing 
the calculations normally associated with that run. 
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2.3.15 REQTRQ 


REQTRQ is called from OUTRE Q to write a file of actuator torques as a 
function of time if this option was chosen by the user. These data may 
then be used to run a response simulation run. 
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2.3.16 REQPLT 


ORIGINAL PS? 

OF POOR QUALITY 


Subroutine REQPLT is called from OUTREQ to write a file of various man- 
ipulator parameters as a function of time during a requirements analysis 
run. This file may then be used to create x-y plots of these parameters 
as a function of time. 


SUBROUTINE REQPLT 


DATA PLOT FILE HEADER INFORMATION FOR EACH PLOT FILE TYPE 


SET TIME TOLERANCE FOR CHECK WHEN BEGINNING FILE WRITE 


RT STRRT TIME 




CS£5Z3B2MagSSMiZ333BG3BSBBBgIZQS3CI2MSMZgEE5S 


BRIEF PLOT PhCKhGE TYPE 


END-EFFECTOR PLOT PRCKPGE TYPE 



JOINT POSITION PLOT PRCKPGE TYPE 


RT STRRT TIME 


REACTION FORCES PLOT PACKAGE TYPE 


CNULL) 


CNULL) 


(NULL) 


CNULL) 


CNULL3 CNULL) 

WRITE TIME. JT. FORCE VECTORS. TORQUE VECTORS 


\ COMBINATION OF PBOVE FOUR PLOT PRCKPGE TYPES 


RT STRRT TIME 




CNULL] 


WRITE TIME. JT. RTMQ. PO«. . PCS. . RNQ. VKL.. . RNO. PCC. . FQRC. - TOR. . 

ACT. TOR. 


WRITE TIME. ENO-EFF. RNG. POS. . POS. . FORCE. TORQUE 


RETURN 


END 




























2.3.17 POSGRDJT 


POSGRDJT computes the position of the intermediate joint in a special 
joint combination called a "Gordy Joint." This position is selected to 
satisfy a constraint on the three joints in this combination. 



SUBROUTINE POSGRDUT 


SETUP X2. X3> Y3. RND HO WITH COORDINATE AXIS | 

VECTORS 

COMPUTE COEFFICIENTS R. B. C RND O USING THESE 

VECTORS 

£ 

D LESS THAN ZERO 

Vs. 

PRINT ■OISCRIMINRNT ERRORS 
URRNXNG 

(NULL) 

s 

DENOMINATOR 

A = ZERO 


DTI— 1 = PI 

DTH - 2 TIMES 
RTfiN C CD— B) /FO 

ADD DTH TO DISPLACEMENT OF 

INTERMEDIRTE JOINT 



COMPUTE NEW 

POSITION 


£ 

DIRECTION OF RESULT IS WRONG 
\ — ■ 

Wm£* 


SUBTRRCT DTH BACK OFF OF 

JOINT DISPLACEMENT 



DENOMINATOR 


A = ZERO 

A 


DTH = 0.0 

DTH — 2 TIMES 
ATAN C C— D— BD /AD 


ADD DTH TO JOINT 

DISPLACEMENT 



COMPUTE NEW 


POSITION 
















2.3.18 JTPOS 


JTPOS is an iterative routine for finding a set of joint angles corres- 
ponding to a desired hand position and orientation. The error DPOS in 
position is calculated and then ORERR is called to find the orientation 
error and transform it into a rotation vector. This rotation vector is 
combined with DPOS, giving DP. The Jacobian relating hand motion to 
joint motion is computed and the set of six linear equations [J](DTG) - 
(DP) is solved for the joint updates DTH. This procedure is repeated 
until the desired position is obtained. 


SUBROUTINE JTPOS 

INITIALISE TOLERANCES. LIMITS RND 

SCALING FACTORS 


COMPUTE POSITION ERROR DR 


COMPUTE ORIENTATION ERROR DPHI 

\ TOTRL POSITION ERROR NOT 

T\ LESS THRN TOLERRNCE 

/> 

CRLL URCOB TO EVRLURTE 
URCOBIRN 


CRLL SLVLIN2 TO COMPUTE 
UOINT UPDATES DTH 

m 

RDD DTH TO CURRENT UOINT 
POSITIONS 

3 

EVRLURTE NEW END-EFFECTOR 

POSITION 



DO UNTIL TOTAL POSITION ERROR LESS 
THAN TOLERRNCE 
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2.3.19 CVTIN 


CVTIN transforms link inertia matrices from local coordinates into their 
equivalent representation in world coordinates for use in dynamic analy- 
sis. 



SUBROUTINE CVTIN 
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2.3.20 SPRGFOR 


Subroutine SPRGFOR is called from FORCE when the manipulator end- 
effector is modeled as a compliant entity. This routine calculates the 
forces and torques at the end-effector reference point caused by its 
having compliance. 


SUBROUTINE SPRGFOR 


CRLL MRTMPY TO GET SPRING REF 
POINT IN WORLD COOR 


RDD WORLD ORIGIN TO END EFFECTOR 

ORIGIN VECTOR 


FIND DISTRNCE BETWEEN REF RND 
RCTURL LOCRTION 


ROTRTIONRL STIFFNESS INCLUDED 


CRLL ORERR TO CRLC DELTA 
ORIENTATION 


CRLL MRTMPY TO CRLC RESULTING 
FORCES RND TORQUES 


CNULL3 


CRLC FORCES RND TORQUES DUE TO 
LINERR DISPLRCEMENTS 


FIND FORCES RND TORQUES RT THE END 
EFFECTOR REF POINT 

















2.3.21 CNSTFOR 




SUBROUTINE CNSTFOR 

COMPUTE F. FORCE ON END EFFECTOR 
DUE TO CONSTRAINT 

COMPUTE T. TORQUE RBOUT END 
EFFECTOR REF POINT DUE TO 
CONSTRRINT 

HDD F AND T TO FEND AND TEND 


RETURN 


END 


CNSTFOR is called from subroutine FORCE to compute the force on the end- 
effector and the torque about the end-effector reference point attribut- 
able to external constraints. These values are then added to the vari- 
ables FEND and TEND. 



2.3.22 PTACC 


PTACC computes the acceleration of any point in any link of either arm. 
It uses the angular velocity and acceleration of the link to find the 
acceleration of the point relative to the acceleration of the link's 
origin and adds this to the acceleration of this link origin. 


SUBROUTINE PTRCC 


OMEGR = LINK RNGULRR VELOCITY 

VEC = VECTOR 

FROM LINK ORIGIN TO 
POINT 

RLPHR = LINK 

RNGULRR RCCELERRTION 

WCV = 

OMEGR 

CROSS VEC 

WCU1CV = 

OMEGR 

CROSS NCV 

RLCV = 

RLPHR 

CROSS VEC 

RESULT = RLCV 
RCCELERRTION OF 

-+- WCUICV 
LINK ORIGIN 
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2.3.23 POSSENS 


POSSENS is called from subroutine CONTROL when one of the feedback con- 
trol laws is being used to drive a response simulation run. This rou- 
tine obtains the discrete representation of the actual joint positions 
and also determines the actual joint velocities and accelerations. 


• SUBROUTINE POSSENS 



DISCRETIZE UQINT POSITION DATA. 

STH 

JOINT VELOCITIES. STHD = 
CSTH— OLDSTH 1 ) / CTIME— SIGLRST3 

JOINT ACCEL. . STHDD = 
CSTH — 2. 0*0LDSTH 1 +OLDSTH23 / 
CTIME — SIGLRST) -*-h-2 

OLDSTH2 = OLDSTH 1 

OLDSTH 1 = STH 

□O UNTIL NJ . EQ. NUMBER OF JOINTS 
IN CURRENT RRM 

RETURN 

END 
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2.3.24 SIMPRT 


SIMPRT outputs the condensed or full data printout to file. It prints 
the position, velocity, and acceleration data for the arm at the time 
when called. If Input flag IPRINT equals 2, a limited amount of infor- 
mation is printed (only TH, THD, THDD, and TDR). 


SUBROUTINE SIMPRT 


DO FOR ERCH FIRM OF ROBOTIC SYSTEM 


WRITE CURRENT SIM. TIME 


CONVERT THETR TERMS TO I/O UNITS 
FOR RL_1_ JOINTS OF RRM 

WRITE UT. RNGLES. VEI_. . RCCEL. 
RND DRIVE TORQ. 

RID CONTROL- y/. 

WRITE RID „ „ . - 

VARIABLES CNUI_L) 

FULL- PRINTOUT OPTED y/. 

WRITE EFFECTIVE 
INERTIR MRTRIX 

[NULL] 

WRITE JOINT /LINK 
PRRRMETERS 

RETURN 

END 
















2.3.25 SIMPLT 


SIMPLT allows the user to write a plot file for output. The user is 
asked to choose from among several different plot package options. The 
chosen package determines which response simulation parameters are 
written to the plot file. 

Option 1, the BRIEF PLOT PACKAGE, writes joint angular displacements, 
joint angular velocities, joint angular accelerations and drive torques. 

Option 2, the END-EFFECTOR PLOT PACKAGE, writes end-effector transla- 
tional position, force vector at the end-effector and torque vector at 
the end-effector. 


Option 3, the JOINT POSITIONS PLOT PACKAGE, writes translational joint 
positions. 

Option 4, the REACTION FORCES PLOT PACKAGE, writes force joint vectors 
and torque joint vectors. 

Option 5, the COMBINATION PLOT PACKAGE, writes all of the above— joint 
angular displacements, translational joint positions, joint angular 
velocities, joint angular accelerations, force joint vectors, torque 
joint vectors, drive torques, end— effector translational position, force 
vector at the end-effector and torque vector at the end— effector . 


Option 6, the PID CONTROL PLOT PACKAGE, writes amplifier voltages, joint 
reference positions, joint position errors, end-effector reference posi- 
tion and end— effector position error* 

Option 7, the FORCE/TORQUE PLOT PACKAGE, writes amplifier voltages, re- 
ference position, reference force, end-effector translational position, 
force vector at the end— effector , torque vector at the end— effector , 
error in position and error in force/torque . 

SUBROUTINE SIMPLT 


SET PLOT HERDER DRTR RECORDS FOR ERCH PLOT TYPE ~] 


SIM. RT STRRT TIME 




WRITE 



CNULL3 


1— 11 




BRIEF PRCKRGE CHOSEN 






(NULL] 


1 [Lead 





END-EFFECTOR PRCKRGE CHOSEN 

- 




CNULLD 


ES5Z3I 

TIME, ROS. . FORC. . RND TORO. RT END-EFFECTOR 




JOINT POSITION PRCKRGE CHOSEN 



| WRITE HERDER RECORO IP 3TRRT TIME 1 


CNULL3 






RERCTION FORCES PRCKRGE CHOSEN 



WRITE HERDER RECORD IF STRRT TIME 


(NULL) 


WRITE TIME. FORC. RND TORQUES RT JOINTS 




COMBINATION PRCKRGE CHOSEN 



| WRITE HERDER RECORD IF STRRT TIME 


CNULL] 


WRITE 

TIME. RNO RLU OF THE RBOVE PRRRMETER3 




PID CONTROL. PRCKRGE CHOSEN 



1 WRITE HERDER RECORD IF STRRT TIME i 

(NULL) 


WRITE 

TIME. JOINT VOUTS.. REF. RNOLE RNO JT. PCS. ERRORS 



WRITE 

TIME* END-EFFECTOR REF. POS. . PCS. RND ROT. ERR. 




FORCE/TORQUE CONTROL. PRCKRGE 

CHOSEN 


WRITE HERDER RECORD IF STRRT TIME 



WRITE TIME. RND JOINT VOLTS. 

CNULL] 


lES 

rxMK. rkf. fob. . fbf. for. /fot. vkctl. bnd-bff. fob. /fou. , fno 




RETURN 

END 
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2.3.26 FORTOR 


FORTOR is called from subroutine CNTRSIG 
is used to drive a response simulation. 


if manual force/torque control 
This routine calculates the 


joint position error vectors caused by the error in the position- 


controlled components of end-effector motion. 


SUBROUTINE FORTOR 



CALL ORERR TO DETERMINE THE 
ORIENTATION ERROR VECTOR 

REMOVE TORQUE CONTROLLED COMPONENTS 
FROM ORIENTATION ERROR VECTOR 

COMBINE POS. RND OR. ERROR VECTORS 
INTO THE VECTOR DELP 

CALL JRCOB TO CALCULATE THE 

JACOBIAN 

CALL SLVLIN2 TO SOLVE FOR DELTA 
JOINT POSITIONS 

SET REFERENCE JOINT POSITIONS 

RETURN 


END 


(NULL) 















2.3.27 FORREF 


FORREF is called from subroutine CNTRSIG when manual force/torque con 
trol is used to drive a response simulation. Individual joint torque 
error vectors are calculated from the end-effector force error and tor- 
que error vectors. 



SUBROUTINE FORREF 

CflLCULRTE END EFFECTOR FORCE ERROR 

COMPONENTS 


CALCULATE END EFFECTOR TORQUE 
ERROR COMPONENTS 


STORE ERROR COMPONENTS IN VECTOR 

DELFT 


CALCULATE REFERENCE FORCE/TORQUE 

VECTOR 


CRLL JACOB TO DETERMINE THE 
JACOBIAN. RURCOB 


CRLCULRTE UOINT TORQUES. TORUNT = 
TORJNT-OELFT*RJACOB 


RETURN 


END 
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2.3.28 CMP CTRL 


CMPCTRL is called from CNTRSIG when active compliance control is used 
in a response simulation run. This subroutine first calculates end 
position deltas (ref-actual), joint control torques, and joint torque 
deltas (control-sensed). The thetas are put through a derivative con- 
trol block to get joint torques. The joint torque deltas are put 
through a lead-lag filter in parallel with an integrating control block. 
The joint control torques are summed with the other processed signals 
to get a total joint torque. This is then converted to motor amplifier 
input voltages. 


SUBROUTINE CMPCTRL. 


SET 

TVCVT. JOINT TORQUE TO VOLTS 
CONVERSION FRCTOR 

CALL URCOB TO 

CRLCULRTE THE URCOBIRN. 
RURCOB 

DETERMINE RUTRRNS. THE TRRNSPOSE 

URCOBIRN 

OF THE 

CRLL 

MRTMPV 

TO FIND TOR. THE INPUT 
TORQUES 

CALL 

MRTMPV 

TO FIND TBIRS. THE 
TORQUES 

BIRS 

JOINT 

CONTROL TORQUES. TCTRL = 

TBIRS 

TOR 

DETERMINE TSENS. SENSED FORCES RND 

TORQUES 

TORQUE 

DELTRS 

. DELTOR = TCTRL 

TSENS 

CALCULATE RJTORQ. JOINT RCTURTOR 

TORQUES 

DRIVE 

CONVERT JOINT 

TORQUES TO INPUT VOLTRGES .... 

RETURN 

END 
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2.3.29 PIDCON 


Subroutine PIDCON is called from CONTROL when a control law is used to 
drive a response simulation run. The routine takes the vector of joint 
position errors and, simulating a PID control loop, calculates joint 
actuator voltages. 


SUBROUTINE PIDCON 

SET JOINT RCTURTOR TORQUE TO VOLTS 

CONVERSION FRCTOR 


CRLCULRTE UOINT POSITION ERROR. 
□ELTH — THREF - STH 


CRLCULRTE ERRINT. THE ERROR 

INTEGRRL 


CRLCULRTE RUTORQ. UOINT RCTURTOR 

TORQUES 


CONVERT RCTURTOR TORQUES TO 

VOLTRGES 


RETURN 










2.3.30 PIDFOR 


Subroutine PIDFOR is called from CONTROL when force/torque control is 
being used to drive a response simulation run. This routine calculates 
the joint actuator voltages caused by the force-controlled components 
of manipulator motion. 


SUBROUTINE PIDFOR 


COMPUTE 
TORQUE TO 

TVCVT. UOINT ACTUATOR 
VOLTS CONVERSION FRCTOR 

COMPUTE 

FERRINT. FORCE ERROR 
INTEGRAL 

CALCULATE 

RUTORQ. UOINT ACTUATOR 
TORQUES 

CONVERT UOINT ACTUATOR TORQUES TO 

VOLTAGES 


RETURN 


END 
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2.3.31 EFINRT2 


EFINRT2 computes the effective inertia matrix (in joint coordinates) for 
a manipulator. The effective inertia matrix is an NxN matrix that gives 
the joint torques attributable to joint accelerations. The (m,n) term 
corresponds to joints m and n and depends on the mass of the arm from 
link n to the end-effector so the program evaluates composite masses, 
centroids and inertia distributions for these “composite masses." Each 
term of the effective inertia matrix is then evaluated as a combination 
of dot products and cross-products among the joint axis directions and 
locations and the mass parameters of the composite links (see Study Re- 
sults volume). 


SUBROUTINE EFINRT2 



INITIALIZE TOTM. TOTIN RNO TMCG CTOTRL. 

INERTIR RNO MBSS TIMES CG l_ 

MRSS. 


DO 

FOR N = LRST LINK TO FIRST 




TOTM — TOTM MRSS OF LINK 

N 


FIND 

NEW COMPOSITE CENTROID BY PDDXNG PROPERTIES OF 

LINK N 


FIND NEW COMPOSITE INERTIR MRTRIX 

ST 

N NOT EQURL BRSE 

T 



N IS R REVOLUTE JOINT 


DO FOR ALL JOINTS M 
FROM N TO BRSE 
M IS R 
REVOLUTE 
JOINT 



FIND ROTBTIONRU. 
TERM OF 
EFFECTIVE 
INERTIR 

COMPONENT CM. NI 


EFFECTIVE 
INERTIR 
COMPONENT 
CM. NJ — ZERO 


DO FOR RLL 
JOINTS M 
FROM N TO 
BRSE 


EFFECTIVE 
INERTIR 
COMPONENT 
CM. NJ - ZERO 


DO FOR RLL JOINTS M FROM N TO BRSE 


FIND TRBNSLRTIONBL 

CONTRIBUTION 

TO 

1 EFFECTIVE INERTIR COMPONENT 

CM. N3 

AND 

EFFECTIVE INERTIR TERM 

CN* M3 - 

TERM 

CM. NJ 
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2.3.32 NLINK 



Subroutine NLINK is called from DERIV during response simulation to com- 
pute the joint accelerations THDD. It sets the joint accelerations to 
zero, then uses the requirements analysis tools to compute effective 
joint torques, EFINRT to compute the effective inertia, DRTORQ to find 
the driving torque and SOLVE to solve the set of linear equations for 
THDD. 


SUBROUTINE NLINK 


CALL ACTIVPIM TO ACTIVATE PEG— IN— HOLE 

CONSTRAINTS 

ZERO RNG. RCC. VRLUES RND CONSTRRINT 

RERCTION FORCES 

CRLL OYNAM TO COMPUTE EFFECTIVE 

TORQUES 



CALL DRTORQ TO COMPUTE DRIVING 

TORQUES 



COMPUTE ACCELERATION TORQUE RS 
TOR. - RCT. TOR. FOR ERCH JT. 

□RIVE 

/RRM 



CRLL EFINRT TO COMPUTE EFFECTIVE 

INERTIR 



DO FOR ERCH RRM 




DO FOR ERCH JT. IN RRM 



■ 

COMPUTE TOTRL LINK INERTIR MRTRIX IN 

UT. COORDS. 

■ 

■ 

COMPUTE INERTIR MRTRIX FOR ERCH 
OF RRM IN INERTIRL COORDS. 

UT. 

§ 

■ 

CRLL SLVTHDD TO SOLVE FOR UT. 
RCCELERRTIONS 

■ 

RETURN 

END 
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2.3.33 SIMLMT 


SIMLMT is called by DERIV and first checks the joint displacements 
against their limits. If any limits are exceeded, the joint position 
is set to that limit and the joint rate and acceleration are limited to 
zero. Similarly, the rate limits are checked and if any are exceeded, 
the corresponding rate is set to that limit and the acceleration is 
bounded by zero. IMOD is set if any positions or rates are modified. 



SUBROUTINE SIMLMT 


IMOD = O 


DO FOR EACH ARM 


DO FOR ERCH UOINT N 


JOINT DISPLACEMENT EXCEEDS UPPER SOUND 


SET DISPLACEMENT TO UPPER BOUND 


limit velocity wo «r cm wmmrm* to lem thrn or soum. to ■■no 

CNULLD 

IMOD - 1 


UOINT DISPLACEMENT EXCEEDS LOWER BOUND 

SET DISPLACEMENT TO LOWER BOUND 


LIMIT VELOCITY WO MCBLEWmON TO ORERT ER THRN OR EOURL TO ^ 

EERO 

(NULL] 

IMOD - 1 


UOINT RATE EXCEEDS UPPER BOUND 

SET RRTE TO UPPER BOUND 


LIMIT RCCELERRTION TO LESS THRN OR EOURL TO EERO 

CNULL) 

IMOD = 1 


^ UOINT RRTE EXCEEDS LOWER BOUND 

SET RRTE TO LOWER BOUND 

. 

LIMIT ACCELERATION TO GRERTER 

1 

THRN OR EQUAL TO ZERO 


IMOD = 1 
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2.3.34 STOPFR 



Subroutine STOPFR is called from INTGRT to simulate static friction in 
the joints during a response simulation run. If the joint velocity at 
the previous time step is not equal to zero and the sign is the opposite 
of the sign of the current time step, the current velocity and position 
delta are set to zero. 


SUBROUTINE STOPFR 


1 

■ 

\ OLD JOINT VELOCITY 

t\ zero 

. NE. 

A 

A 


I 

DETERMINE SIGN OF OLD 
JOINT VELOCITY. SO 

CNULL3 

1 


DETERMINE SIGN OF CURRENT 
JOINT VELOCITY. SC 


1 

SO . NE. SC 


Z 

V, 

« 

1 

SET CURRENT VELOCITY 
TO ZERO 

CNULLJ 


1 

SET POSITION DELTR 
TO ZERO 


DO UNTIL N . EQ. NUMBER OF 

JOINTS 


DO UNTIL KRRM . EQ- NUMBER OF 

ARMS 


RETURN 

END 
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2.3.35 ACTIVPIH 


ACTIVPIH sets up the flags and variables activating a peg-in-hole con- 
straint if such a constraint is included. It sets up four point con- 
straints — two each (in orthogonal directions) at the top of the hole 
and at the tip of the peg. 



. SUBROUTINE HCTIVPIH 

DO FOR ERCH RRM 



IPIH NOT EQURL ZERO 


^ - — — 1 ■ 

PUT TOOL. REFERENCE POINT LOCRTXON INTO PEGLOC 

PUT (MINUS) 

HOLE-AXIS DIRECTION INTO PEGDIR 


DEL. 1 EQUALS UNIT VECTOR RLONG X-RXIS CROSS 

PEGDIR 

^ MAGNITUDE OF DELI NERR ZERO 

m 

DELI — UNIT VECTOR 
ALONG V— AXIS 

NORMALIZE DELI 

□EL2 — UNIT VECTOR 
ALONG 2— AXIS 

DEL2 - DELI CROSS 
PEGDIR 

INITIALIZE FLAGS FOR 4 
DOUBLE-SIDED POINT CONSTRAINTS 

POINT 

1 RT PEG TIP PLUS HOLE 
RRDIUS RLONG DELI 


POINT 

1 RT PEG TIP PLUS HOLE 
RRDIUS RLONG DEL2 


POINT 1 RT HOLE ENTRRNCE PLUS 
HOLE RRDIUS RLONG DELI 


POINT 1 RT HOLE ENTRRNCE PLUS 
HOLE RRDIUS RLONG DEL2 
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(NULL) 






2.3.36 DRTORQ 


The DRTORQ routine calculates the torque output from each joint motor 
by using a control algorithm strategy or reading them from a file. The 
calculations are based on the torque constant for each joint and the 
armature current. 


SUBROUTINE DRTORQ 


3 


IK 


RERD TORQUE FLRG. ITORQ. = 1 


TIME . QE. NEXT TIME RT MMXCH TO RERO TORQUE FILE 


|rero time from torque file into tnext 


rz 

KRRM *» O 

_ 

■ 

INCREMENT KRRM 

1 

■ 

TORL o^orr TIMOUD - TOWN CNKXT TOWBUP FOR BWCH JT. 

=g 

1 



DO 

UNTIL. KRRM — NRRM 



CALL LININT TO COMPUTE NEW TORQUE CONTROL 
SIGNAL TIME COEFFS. 


KRRM 


INCREMENT KRRM 


0 

II 

z 

■ 

INCREMENT N 


TDR CN. KRRM3 . DRIVING TORQUE 


MB 

INTERPOLATION BETWEEN TORL RND 

TORN 

DO 

UNTIL. N - NUMBER OF JOINTS IN 

KRRM 


DO UNTIL. KRRM 


NRRM 


^ UJ 

c_> * 


c_> GC 


s i 




2 . es 

3 


RETURN 

END 
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2.3.37 EFINRT 


EFINRT calls EFINRT2 to compute the effective inertia matrix of each 
manipulator and puts these results into diagonal blocks of a larger 
matrix. 



SUBROUTINE EFINRT 


INITIflLIEE INDEX — ONE 

DO FOR ERCH RRM 


CALL EFINRT2 TO COMPUTE 
EFFECTIVE INERTIR MRTRIX FOR RRM 

PUT INTO RESULT STARTING RT 
COMPONENT CINDEX. INDEX) 

INDEX = INDEX ■+■ NUMBER OF JOINTS 

IN RRM 
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ORIGINAL PAGE sS 
OF POOR QUALITY 


2.3.38 SLVTHDD 

SLVTHDD solves for unknown joint accelerations and constraint reaction 
forces for a given arm state and joint driving forces. All zero- 
velocity joints are assumed to have zero acceleration. If the friction 
forces needed to produce zero acceleration are greater than the static 
friction force, the acceleration is assumed finite and the equations are 
re-solved. Similarly, the constraints are assumed active and if the re- 
sulting constraint force is in the wrong direction the constraint be- 
comes inactive and the equations are re-solved. This process is re- 
peated until all conditions on the friction forces and constraints are 
satisfied. 


SUBROUTINE SLVTHDD 


DO 

FOR ERCH JOINT 





— 1 


JOINT RRTE - 

ZERO RND 

STRTIC FRICTION NONZERO 






FLZV - 

TRUE 

1 FLZV - 

FALSE 



1 

INITIRLIZE FLHR - FLZV 

■ ! 


NO 

CONSTRAINTS OR ZERO-VELOCITY JOINTS 

EXIST 




CO 

CRL.L. JRCOB TO EVALUATE THE JRCOBIRN j 

CD 

hH 


SETUP PHXT WITH COEFFICIENTS FOR NQNEERQ ACCELERRTION TERMS 

1 

or 


TTEMP - TEFF 

cn 

U_l 


KSSHHHHI 

CONSTRRINTS EXIST 





U_J 



(NULL) 




C_3 

CJ 


SOLVE FOR UNKNOWN RCCELERRTZONS PND CONSTRPINT REACTION FORCES 

cc 


□O FOR ERCH CONSTRRXNT FORCE 







ksvmiiSsacasQ 





. \ 

M 



[NULL3 



/ : 

O 


«SESiE33SC53uEB3C iC333ESiESaC53 i i i ■ — 






MAKE CONSTRAINT ACTIVE | CNULL3 

1 

CD 

LU. 


\RLL CONSTRRXNT CONDITIONS SATISFIED 



2 

LU 


COMPUTE FRICTION TORQUES AT JOINTS WHERE FLER - 

TRUE 






DO FOR ALL JOINTS 

FOR WHICH FLZV - TRUE 




CO 

a 


RCCELERflTION IS NONZERO 

/r 

m 




\y rWICTXON / 

\ FRICTION FORCE EXCEEDS / 


j 

LlJ 



T\ FRICTION COEFFICIENT /p 

■ 

ZD 



I \ ncrm ftirtxon S F 

SET FLZR - 

. — . 


' — ' 

<ZD 

CO 


SET 


FALSE 

1 1 1 






FL2R — 

(NULL) 

ar friction forck courl 

ZZD 




t 




TO COULO0 FRICTION 





1 


TRUE 


COKFFIdBNT 

' — * 




cn 

C-3 

DO 

UNTIL RLL FORCE RND CONSTRRXNT CONDITIONS 

SATISFIED 
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2.3.39 LININT 


Subroutine LININT is called to set up the coefficient for performing 
linear interpolation between two vectors. 
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2.3.40 LDVOLT 


Subroutine LDVOLT is called from DRTORQ when a file of actuator voltages 
is to be read in and used to drive a response simulation run. At the 
correct time the routine reads time and voltage from an existing file. 
LININT is called to interpolate the best voltage for the current simu- 
lation time. The control voltage is then calculated from this. 


SUBROUTINE LDVOLT 

* 

\ 

IRDCNTL - EQ. 1 




V 

CORRECT TIME TO RERD NEW 

DRTR 



RERD IN TIME 



■ 

■ 

RERD HRRDWRRE 
VOLTAGES 

=j 

1 

1 

DO UNTIL. N . EQ. NUMBER 
OF JOINTS 



□O UNTIL. KRRM . EQ. 
NUMBER OF RRMS 



CALL 

LININT TO INTERPOLATE DRTR FOR 
CURRENT SIMULATION TIME 



CALCULATE SIMULATION CONTROL 

VOLTAGES 


RETURN 

END 
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2.3.41 CALCI 


The CALCI subroutine calculates the amplifier current 
of the joints in the system given the motor parameter 
state velocity. 


values for each 
values and the 



SUBROUTINE CRLCI 


KRRM. RRM COUNTER = O 

DC 

OF 

INCREMENT KRRM 

N. JOINT COUNTER — O 

DC 

FC 

RRMRTURE I CN. KflRM) - CRMP 
GRIN-h-VOLTRGE — BRCK 
EMF*JT. VEL. 3 /RRM RES. 

UNTIL N — NUMBER OF UOINTS 
)R KRRM 

UNTIL KRRM - NRRM* TOTRL NUMBER 
RRMS 

RETURN 

END 
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2.3.42 SOLVE 


SOLVE is used to solve a set of N linear equations in N unknowns. It 
sets up an identity-augmenting matrix, calls GAUSS to invert the origi- 
nal matrix and then multiplies this inverse tiroes the right-hand side 
of the equations to obtain the resulting solution. 


SUBROUTINE SOLVE 

PUT ORIGINAL MATRIX INTO C 


FORM IDENTITY RUGMENTING MRTRIX IN 

RUG 


CALL GRUSS TO PERFORM ELIMINATION. 
PUTTING INVERSE OF C INTO RUG 


MULTIPLY RUG BY RIGHT— HRND-SIDE OF 
ORIGINAL EQUATIONS TO GET X 









2.3.43 GAUSS 


GAUSS performs Gauss-Jordan elimination with partial pivoting on an aug- 
mented matrix system to reduce the system to row-echelon form during the 
matrix inversion process. The largest value remaining in a column is 
used as the pivot value for that column during reduction. 



SUBROUTINE GAUSS 

START WITH ORIGINAL MATRIX IN R AND 
IDENTITY AUGMENTING MATRIX B 


DO FOR EACH COLUMN I 


FIND ROW U WITH LRRGEST 

VRLUE 

R CU. 11 


^MAGNITUDE R CU. 11 NOT 

EQURL 

ZERO 

/w 


DO FOR EACH PREVIOUSLY REDUCED ROW 


REDUCE 

COLUMN I OF R TO 

ZERO 

■ 

PERFORM 

SRME ROW OPE RRTXONS 

ON B 

1 

DO FOR 

ERCH ROW NOT YET REDUCED 

REDUCE 

COLUMN I OF R TO 

ZERO 


PERFORM 

SRME ROW OPERATIONS 

ON B 

■ 

REDUCE PIVOT ROW OF R TO 
STRRT WITH 1 

PERFORM SRME OPERRTION 

ON B 


MOVE PIVOT ROW UP TO ROW I 
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(NULL) 

















2.4.1 SLVLIN2 


SLVLIN2 finds an optimal solution X to a linear set of equations AX = B 
where the magnitude of each component of X is bounded -XLIM(N)< * X(N) 
<£,** XLIM(N). The program first sets up matrices D, which forms an ortho- 
gonal basis for the reachable space of A, and C, which provides the con- 
version from D space to AH space. The matrix AN is also set up; it con- 
tains vectors in the null space of H along with the initial solution VH 
and is used as the tableau for linear programming. Once an initial so- 
lution is found, linear programming by a modification of the Simplex me- 
thod is performed; the magnitude of the result is maximized subject to 
the constraints on X. This solution is then scaled to give the final 
solution. 


SUBROUTINE SLVLIN2 


SCALE COLUMNS 

OP R (GIVING RH3 SO 

THRT 

LIMITS 

ON X RRE ONE 

PUT 

FIRST COLUMN 

OF 

RH 

INTO D 


DO FOR ERCH REMAINING COLUMN OF AH 


SUBTRACT COMPONENTS OF 

COLUMN ALONG 

COLUMNS OF D 

Hi 

RESULTING COLUMN 

VECTOR IS 

NOT ZERO 

^ 


MAKE RESULTING COLUMN NEXT 

COLUMN OF O 


PUT COLUMN 
INTO NULL 
SPACE MATRIX AN 

FORM NEXT COLUMN OF 

C MRTRIX 



USE 

D RND C TO SOLVE LINERR EQUATIONS 


t\ 

EQUATIONS SOLVED 

EXRCTLY 

/F 

IAPR = O 

IAPR = 1 

\ NULL SPACE EXISTS RND B 
T\ SPRCE 

NOT IN THRT 

3 


GENERATE INITIAL BASIC 
FEASIBLE SOLUTION 


DO WHILE FURTHER OPTIMIZATION 
IS POSSIBLE 


CALL REPCOL TO FURTHER 
OPTIMIZE 






CD 



| 

U-J 

Li — l 



r 

CL 

CE 

cc 

CD 

LU 

1 *■ 

CO 
1 — 1 

CL 

CO 

LlJ 
U-J 
C 3 

\ I 

cd 

CO 

o 

1 

CD 


H— 
1— 1 

X 

LLJ 


CO 

CO 

1 1 

1 



1 

CD 


SCRLE RESULTS 


CUSING XLIM) TO GET FINRL 
SOLUTION 
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2.4.2 REPCOL 


REPCOL replaces column ICOL of matrix A where A represents the tableau 
for a linear programming problem, X represents the variables, and the 
limits on each variable are plus or minus one. REPCOL first finds the 
largest allowable change in the free variable (the variable that corres- 
ponds to the column being replaced), and the new constraint variable 
that limits this change. The solution is updated and tableau A is modi- 
fied to reflect this change of constraint variables. 


SUBROUTINE REPCOL 


FIND WHETHER TO INCRERSE OR DECRERSE 
VRRIRBLE CORRESPONDING TO ICOL 

FIND ROW CIROW) WHICH RLLOWS 
SMALLEST CHRNGE IN THRT VRRIRBLE 

UPDRTE X VECTOR TO REFLECT THRT 
CHRNGE OF VRLUE 

DO FOR ERCH ROW N OF R 


R CN. ICOL3 = 
n CN. ICOL) /R CIROW. ICOL) 

DO FOR ERCH COLUMN I EXCEPT ICOL 


DO FOR ERCH ROW N 


R CN. I) — R CN. I) - 
R CN. ICOL) -k-R CIROW. I) 
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2.4.3 ORERR 


Subroutine ORERR is used to find the change in orientation between two 
coordinate systems. The error in orientation is computed and then 
transformed into a rotation (magnitude less than pi) about a unique 
rotation axis. This axis is computed as the cross-product of two of 
the columns of DOR, and the rotation angle PH is computed by (COS(PH) a 
1+.5*(X.DX+Y.DY+Z.DZ), where X.DX is the dot product of the X column of 
ROR with the X column of DOR, etc. 


SUBROUTINE ORERR 

find magnitude of orientation 

CHANGE FOR EACH COORDINATE RXIS 


DETERMINE WHICH RXIS CHANGES THE 

MOST 


CALL CRPD AND COMPUTE DDPHI. R 
SINGLE RXIS OF ROTATION 


COMPUTE PH. THE ANGLE OF ROTRTON 

RBOUT DDPHI 


RETURN 


END 
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2.4.4 OUTUN 


OUTUN is a function that converts a value from internal (metric) units 
to the user-specified input/output units by dividing by a conversion 
factor. 



END 



2.4.5 ICVTATD 


ICVTATD is a function that returns a digitized number when given a real 
value. If the value is outside an allowable minimum or maximum, it is 
set equal to the appropriate limit. The digitized number is then com- 
puted using the following relation: 

B - (VAL - VALMIN ) * (^ITS - 2 <NBITS-1)\ 

(VALMAX - VALMIN) \ / 

where 

NBITS ■ number of bits available for digitized value, 

VALMIN = minimum allowable value for VAL, 

VALMAX = maximum allowable value for VAL, 

VAL 3 output value. 



ICVTATD, the digitized number, is 

set equal to the closest 

integer to B. 


FUNCTION ICVTRTD 


l> 

VRL CRNRLOG VALUE) . LT. 
\ VRLMIN (MINIMUM ALLOWED) 


VRL = VflLMIN 

(NULL) 

— ’ "’■? 

T 

VRL - GT. VRLMRX (MAXIMUM 
\ ALLOWED) 


VRL = VflLMRX 

(NULL) 

R = CVRL-VRLMIN) / (VRLMRX-VRLMIN) 

B = R*2**NBITS-2** (NBITS- 1 ) 


ICVTRTD = CLOSEST 

INTEGER TO B 


RETURN 

END 
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TAble B-VII. - PROGRAMS EMPLOYED IN POSTDRVR 


3.0 

POSTDRVR 

3.1.1 

SIMOTION 

3.1.2 

HDWMOTIN 

3.1.3 

ROBPLT 

3.2.1 

LDTHET 

3.2.2 

POSTGRAF 

3.2.3 

minmax 

3.2.4 

AXES 

3.2.5 

SCAL 

3.2.6 

TICMRK 



3.0 


POSTDRVR 


The program POSTDRVR is the postprocessing function driver. It operates 
in an interactive mode, prompting the user for the postprocessing option 
desired: replay robotic system simulation motion, replay simulation 

versus hardware motion, parameter versus parameter plots, or terminate 
POSTDRVR execution. For simulation replay, option 1, subroutine 
SIM0TI0N, is called. Option 2 provides a comparison of hardware and the 
corresponding simulation motion through subroutine HDWMOTIN. If option 
3 is selected, ROBPLT plots any of the data computed and written to one 
of the seven types of plot file packages during the requirements or 
simulation analysis tools functions. 


PROGRRM POSTDRVR 


SET 

PROCESSOR MODE - 

3- 

FOR POST 

PROCESSING 


SET ERROR 

CODE = 

o 

CALL 

SETLJJ TO SET PROGRRM 

DEFAULT LOGICAL 

UNIT NUMBERS 


PROMPT USER FOR OPERATION MODE- IMODE 


READ USER RESPONSE 

t\ 



IMODE = 

Dll 


/w 

or 

U_J 

CO 

CO 

□a CASE ON OPERATION MODE 



CO 

LLJ 




/ 

1 

C3Z 

CD 

LX- 

C 3 

CE 


DOCASE 


/ 


Q_ 

LxJ 

j 

cn 

err 

err 

□O 

IMODE 

IMODE 
= 2 

IMODE 
= 3 

/ 


or 

CO 

i— i 




/ 

■ 

CL_ 

CALL 

SIMOTION. 
SIM. MOTION 
REPLAY 
ROUTINE 

CALL 

HDWMOTIN. 

HARDWARE 

CFH_l_ ROBPLT. 
PRRRMETER VS. 

WRITE 

ERROR 

MESSAGE 


CE 
C 3 

= 

VS. SIM. 
MOTION 
ROUTINE 

PLOTTING MPOULE 

■ 


DO UNTIL OPERATION MODE DESIRED IS TO TERMINATE POSTDRVR 
EXECUTION 

STOP 


END 
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3.1.1 


SIMOTION 


SIMOTION is called during the postprocessing function to provide a re- 
play of the robotic system motion produced during a previous run of the 
requirements or simulation phase of the analysis tools function. It 
opens the chosen robotic system geometry file and simulation output file 
for each graphics replay, and calls GRAFIX with the displacements at 
each time step to update the system motion display. 



SUBROUTINE SIMOTION 




OR!G*TA^ 

OF. PGOi* 
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3.1.2 HDWMOTIN 



HDWMOTIN is called during the postprocessing function to provide a re- 
play of the robotic system motion produced during the requirements/simu- 
lation analysis tools functions versus the actual motion that occurred 
during the corresponding hardware run. It opens the chosen system geo- 
metry file, simulation output file for graphics replay and hardware file 
containing recorded joint theta values. It calls POSTGRAF with the 
hardware and simulation displacements at each time step to update the 
system motion display. 


SUBROUTINE HDWMOTIN 



set pgoeeaaoR mope to a ron post processing 


CALL ROSY 3 TO REPO TVg ROBOTIC SYSTEM FILE 


PROMPT FOR FILENRMC OF SIMULATION MOTION OUTPUT FILE* SOF 


RgRO SOF FILENFMg 


SET HRRPMPRE THgTfl RgffP FLRQ* IRQTHgT. * 1 


OPEN SOF FILE 


PROMPT FOR FILENRMC OF HPRPURRg THgTfl INPUT 


REffO HnWONn WE THETH FILENAME 


OPEN ^ wn o ufin g THCTB FILE 


REPO INITIAL TIME FROM 


ET THE TIME CHECK INCREMENT POPPETERfi 



TO XNTTIRLXEE QRPRHXCS 



CRLL 



LORO HWRNG RRRRV 

WITH THETR VALUES FOR ERCH JT. OF EACH RRM 

— 

CRLL SETUP TO LORO THE MfiRDWORE POS RND ROT MPTRICES | 

LORO HWPOS RND 

HWROT WITH VRLUES FOR ERCH JT. OF ERCH RRM 



NRRM 







CRLL SETUP 

TQ LORO THE SIM POS RND ROT MRTRICES 




1, ■■ 1 kj 1 I,'— 

— 

CRLL POSTGRAF TO DXSPLRY CURRENT TIME POST PROCESSING GRRPHICS 1 



PA?-: 


OF POOR QUALITY 
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3.1.3 ROBPLT 


The ROBPLT subroutine plots the contents of one of several choices for 
plot package formats on a Hewlett - Packard X — Y plotter or a VAX VT125 
graphics terminal. It uses exclusively the DI3000 plot package. 

ROBPLT requests the user to select the appropriate one of seven plot 
file types that was written at the user's discretion during the require 
ments or simulation analysis tools functions; the brief package , the 
end - effector package, the joint positions package, the reaction forces 
package, a combination of the above four packages, the PID control pack 
age or the force/torque control package. 
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CALL THETA CALCULATION AOUTINE 


3.2.1 LDTHET 



The LDTHET routine loads the theta values for each joint from direct 
read of the hardware control theta values file. It is called from 
HDWMOTIN during the postprocessing function for each simulation time 
step. There is a limit of one theta signal value for each joint that 
can be read. 



2 


KRRM — O 



INCREMENT KRRM ~ 


THCTL tLfiST 

TWCTR9J - 

THETN CNCXT THCTM8 FOR EACH JT. 


Rgffp THtrrm vnmn fwom h 

i j LLM MHL FP-g INTO THETN FOR CRCH JT. 

DC 

UNTIL 

KRRM 

- NRRM 

CRLU- LINXNT TO 

COMPUTE 

NEW THETR CONTROL. SXGNRl- TIME 
COEFF3. 

KRRM = O 


INCREMENT KRRM 


z 

II 

0 

DO 

INCREMENT N 

TH CN. KRRM) . THETR - INTERPOLATION BETWEEN THETL 

RND THETN 

UNTIL N - NUMBER OF UOINTS IN KRRM 


DO UNTIL. KRRM = NRRM 

RETURN 

END 


SUBROUTINE LDTHET 


RERD THETR FLRG. IRDTHET » - NE. 1 


time . GE. NEXT TIME RT WHICH TO RERD THETR FILE 


RERD TIME FROM HRRDWRRE THETR FIL-E INTO TNEXT 
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(NULL) 



3.2.2 POSTGRAF 

Subroutine POSTGRAF provides the motion graphics capability in the post- 
processing function for HDWMOTIN, a replay of the simulation motion ver- 
sus actual hardware motion. The value of the difference in the simula- 
tion and hardware thetas is displayed, along with the environment, ro- 
botic system and load objects. 



ORjQif'jAl. PAtsfc. sS 
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3.2.3 MINMAX 


MINMAX searches the postprocessor plot file for the maximum and minimum 
values to be used in scaling the axes of the plot. The x and y minimums 
and maximums are found for all parameters the user chooses for plotting. 


SUBROUTINE MINMRX 


KOUNT - Q 

INCREMENT KOUNT 

REUCTND PLOT DffTR FILE 

READ HERDER INFORMATION CONTAINING XPLC CTYPE PLOT) , 


NRRM. NJ 



CRlOir^AL 

OF POOR QUALITY 
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3.2.4 AXES 


Subroutine AXES draws the x and y axes 
plotting option of the postprocessor. 


for a plot during the x-y 



SUBROUTINE RXES 


MIN AND 

MAX ZERO CROSSING CHECK FLAGS - O 


XMIN VALUE - O 

Xp 

lx MIN ZERO CROSSING FLRG - 1 ! 

CNUL-L-) 



XMRX VRI-UE — O 

Ze 

lx MAX HERO CROSSING FU=»G - 1 I 

CNUI_l_3 

Es: 

YMIN VALUE - O 



Iv MIN HERO CROSSING FL.RG - 1 | 

CNULL3 



YMRX VALUE — O 

Xr 

IV MRX HERO CROSSING FURG - 1 | 

CNUL-L-3 

v 

' MIN ZERO CROSSING FLAG - 1 _ 

opT V 

CHECK - V MRX VALUE 

/ V MIN VFH-UC I 

CU. 1 T 

ORIGIN 

>S. CHECK IS NEGRTIVE X 

— o. 

SET Y ORIGIN - O. 

V ORIGIN — Y MIN VALUE 

CRI_l_ JMOVE ‘ 

fO MOVE PEN TO tXMIN. YORG3 COOROINRTE 

ICRUU JDRflW TO DRRUi X-AXIS FROM ORIGIN 

TO CXMRX. YORGJ 

X MIN ZERO CROSSING FI_RG =1 /f 


CHECK - X MRX VALUE 

X X MIN VALUE 

x S . 

CP e=a 

CHECK IS NEGRTIVE /p 

^3 H 


X ORIGIN - X MIN 

00 c=> 

SET X ORIGIN - O. 

VALUE 

CRL-L- JMOVE ‘ 

TO MOVE PEN TO CXORG. YMINJ COORDINRTE 

CALL JPAAM TO DRAW Y— RXIS FROM ORIGIN 

TO CXORG. YMRX3 

RETURN 

END 
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3.2.5 SCAL 


For both user-selected automatic or specified scaling of the postpro- 
cessor plot file, routine SCAL is called from the ROBPLT option. It 
chooses the most appropriate scale for the x- and y-axis tic marks. It 
finds the exponent of the scale base, the tic mark spacing and the min- 
imum tic mark value. The minimum value, XI, to be used for the scale, 
and DX, the scale increment between tic marks, are chosen to satisfy 
specific constraints. 


SUBROUTINE SCRL 


RDD 

R SMRLL TOLERRNCE TO THE MINIMUM 
VRLUE. FOR TRUNCRTION 


SUBTRRCT R SMALL TOLERRNCE FROM THE 
MRXIMUM VRLUE. FOR TRUNCRTION 

FIRST TRIAL VRLUE SCALE 
CMRX— MIN3 /NUMBER 

INCREMENT. DX. — 
PARTITIONS 



SET EXPONENT — 

LOG OF DX 


\^EXPO IS NEGRTIVE RND 

NOT RN 

INTEGER 


EXPO = EXPO - 1. 

(NULL) 

SET 

BRSE = lO. ^INTEGER PRRT 

OF EXPO 


V 

RUTOMRTIC SCRLING IS 

SET 

A 

CHOOSE 
5 OR 

OX THE MRXIMUM OR 1. 2. 

lO TIMES R POWER OF lO. 

(NULL) 

SET 

MINIMUM TIC MRRK VRLUE — INTEGRRL 
MULTIPLE OF STEP SIZE 


RETURN 

END 
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3.2.6 TICMRK 


Routine TICMRK actually draws 
the postprocessor function. 


and labels the tic marks for a plot during 



SUBROUTINE TICMRK 


COMPUTE 

X 

AND Y RXIS LENGTHS 


DRRW TIC MRRKS 

OVER RRNGE OF X-RXIS ORIGIN TO 
XMRX VALUE 


DO RT X-RXIS 

TIC MRRK FREQUENCY 


DETERMINE LENGTH 

OF 

THE CURRENT TIC MRRK LRBEL 

1 

LRBEL THE X-RXIS 

TIC MRRKS WITH VALUES 

■ 

DETERMINE LENGTH 

OF 

THE EXPONENT FOR THE X-AXIS 
SCALE 


OUTPUT EXPONENT FOR THE X— RXIS SCALE 

DRRW TIC MRRKS OVER RANGE OF V-RXIS ORIGIN TO 

VMRX VRI_UE 

DO RT Y-RXIS 

TIC MRRK FREQUENCY 


DETERMINE LENGTH 

OF 

THE CURRENT TIC MRRK L-RBEL. 

■ 

LABEL THE V-RXIS 

TIC MRRKS WITH VALUES 

■ 

DETERMINE LENGTH 

OF 

THE EXPONENT FOR THE Y-RXIS 
SCALE 


OUTPUT EXPONENT FOR THE Y— RXIS SCALE 

RETURN 

END 
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